From 399b881026f8f48163a612f8564ba6acc6e95b0d Mon Sep 17 00:00:00 2001 From: Mobilizes Date: Thu, 16 May 2024 22:09:38 +0700 Subject: [PATCH 01/10] feat: adjust positioning function to dynamic kick --- .../locomotion/process/locomotion.hpp | 6 +++++- .../locomotion/process/locomotion.cpp | 20 +++++++++++++++++-- 2 files changed, 23 insertions(+), 3 deletions(-) diff --git a/include/suiryoku/locomotion/process/locomotion.hpp b/include/suiryoku/locomotion/process/locomotion.hpp index 2c3e7a8..c4b73c7 100755 --- a/include/suiryoku/locomotion/process/locomotion.hpp +++ b/include/suiryoku/locomotion/process/locomotion.hpp @@ -70,7 +70,7 @@ class Locomotion const keisan::Angle & max_pan, const keisan::Angle & min_tilt, const keisan::Angle & max_tilt); bool position_kick_general(const keisan::Angle & direction); - bool position_kick_range_pan_tilt(const keisan::Angle & direction, bool precise_kick, bool left_kick); + bool position_kick_range_pan_tilt(const keisan::Angle & direction, bool precise_kick, bool left_kick, bool dynamic_kick); bool is_time_to_follow(); bool pivot_fulfilled(); @@ -163,6 +163,10 @@ class Locomotion keisan::Angle right_kick_target_tilt; std::shared_ptr robot; + + keisan::Angle max_dynamic_range_pan; + keisan::Angle min_dynamic_range_pan; + double mapped_tilt; }; } // namespace suiryoku diff --git a/src/suiryoku/locomotion/process/locomotion.cpp b/src/suiryoku/locomotion/process/locomotion.cpp index 18a140f..ff3bf4f 100755 --- a/src/suiryoku/locomotion/process/locomotion.cpp +++ b/src/suiryoku/locomotion/process/locomotion.cpp @@ -179,6 +179,8 @@ void Locomotion::set_config(const nlohmann::json & json) position_min_range_pan = keisan::make_degree(val.at("min_range_pan").get()); position_max_range_pan = keisan::make_degree(val.at("max_range_pan").get()); position_center_range_pan = keisan::make_degree(val.at("center_range_pan").get()); + min_dynamic_range_pan = keisan::make_degree(val.at("min_dynamic_range_pan").get()); + max_dynamic_range_pan = keisan::make_degree(val.at("max_dynamic_range_pan").get()); } catch (nlohmann::json::parse_error & ex) { std::cerr << "error key: " << key << std::endl; std::cerr << "parse error at byte " << ex.byte << std::endl; @@ -751,12 +753,26 @@ bool Locomotion::position_kick_custom_pan_tilt(const keisan::Angle & dir return false; } -bool Locomotion::position_kick_range_pan_tilt(const keisan::Angle & direction, bool precise_kick, bool left_kick) +bool Locomotion::position_kick_range_pan_tilt(const keisan::Angle & direction, bool precise_kick, bool left_kick, bool dynamic_kick) { + if (dynamic_kick) { + position_max_range_pan = max_dynamic_range_pan; + position_min_range_pan = min_dynamic_range_pan; + } + auto tilt = robot->get_tilt(); auto pan = robot->get_pan(); - bool tilt_in_range = tilt > position_min_range_tilt && tilt < position_max_range_tilt; + if (dynamic_kick && pan < keisan::make_degree(0.0)) + mapped_tilt = keisan::exponentialmap(pan.degree(), 0.0, position_min_range_pan.degree(), position_min_range_tilt.degree(), position_max_range_tilt.degree()); + else if (dynamic_kick && pan >= keisan::make_degree(0.0)) + mapped_tilt = keisan::exponentialmap(pan.degree(), 0.0, position_max_range_pan.degree(), position_min_range_tilt.degree(), position_max_range_tilt.degree()); + + keisan::Angle min_tilt = (dynamic_kick) ? keisan::clamp(keisan::make_degree(mapped_tilt) - position_min_delta_tilt, position_min_range_tilt, position_max_range_tilt) : position_min_range_tilt; + keisan::Angle max_tilt = (dynamic_kick) ? keisan::clamp(keisan::make_degree(mapped_tilt) + position_min_delta_tilt, position_min_range_tilt, position_max_range_tilt) : position_max_range_tilt; + printf("tilt range: %.2f to %.2f\n", min_tilt.degree(), max_tilt.degree()); + + bool tilt_in_range = tilt > min_tilt && tilt < max_tilt; bool right_kick_in_range = pan > position_min_range_pan && pan < -position_center_range_pan; bool left_kick_in_range = pan > position_center_range_pan && pan < position_max_range_pan; bool pan_in_range = precise_kick ? (left_kick ? left_kick_in_range : right_kick_in_range) : (right_kick_in_range || left_kick_in_range); From 4f6810d40fc30f97eb6189c808c99ba76f719fdc Mon Sep 17 00:00:00 2001 From: hiikariri Date: Wed, 29 May 2024 22:31:05 +0700 Subject: [PATCH 02/10] feat: differ tilt range for dynamic kick --- include/suiryoku/locomotion/process/locomotion.hpp | 2 ++ src/suiryoku/locomotion/process/locomotion.cpp | 9 +++++++-- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/include/suiryoku/locomotion/process/locomotion.hpp b/include/suiryoku/locomotion/process/locomotion.hpp index c4b73c7..a6cb7dc 100755 --- a/include/suiryoku/locomotion/process/locomotion.hpp +++ b/include/suiryoku/locomotion/process/locomotion.hpp @@ -166,6 +166,8 @@ class Locomotion keisan::Angle max_dynamic_range_pan; keisan::Angle min_dynamic_range_pan; + keisan::Angle max_dynamic_range_tilt; + keisan::Angle min_dynamic_range_tilt; double mapped_tilt; }; diff --git a/src/suiryoku/locomotion/process/locomotion.cpp b/src/suiryoku/locomotion/process/locomotion.cpp index ff3bf4f..9aeac57 100755 --- a/src/suiryoku/locomotion/process/locomotion.cpp +++ b/src/suiryoku/locomotion/process/locomotion.cpp @@ -181,6 +181,8 @@ void Locomotion::set_config(const nlohmann::json & json) position_center_range_pan = keisan::make_degree(val.at("center_range_pan").get()); min_dynamic_range_pan = keisan::make_degree(val.at("min_dynamic_range_pan").get()); max_dynamic_range_pan = keisan::make_degree(val.at("max_dynamic_range_pan").get()); + min_dynamic_range_tilt = keisan::make_degree(val.at("min_dynamic_range_tilt").get()); + max_dynamic_range_tilt = keisan::make_degree(val.at("max_dynamic_range_tilt").get()); } catch (nlohmann::json::parse_error & ex) { std::cerr << "error key: " << key << std::endl; std::cerr << "parse error at byte " << ex.byte << std::endl; @@ -758,6 +760,8 @@ bool Locomotion::position_kick_range_pan_tilt(const keisan::Angle & dire if (dynamic_kick) { position_max_range_pan = max_dynamic_range_pan; position_min_range_pan = min_dynamic_range_pan; + position_min_range_tilt = min_dynamic_range_tilt; + position_max_range_tilt = max_dynamic_range_tilt; } auto tilt = robot->get_tilt(); @@ -768,9 +772,10 @@ bool Locomotion::position_kick_range_pan_tilt(const keisan::Angle & dire else if (dynamic_kick && pan >= keisan::make_degree(0.0)) mapped_tilt = keisan::exponentialmap(pan.degree(), 0.0, position_max_range_pan.degree(), position_min_range_tilt.degree(), position_max_range_tilt.degree()); - keisan::Angle min_tilt = (dynamic_kick) ? keisan::clamp(keisan::make_degree(mapped_tilt) - position_min_delta_tilt, position_min_range_tilt, position_max_range_tilt) : position_min_range_tilt; - keisan::Angle max_tilt = (dynamic_kick) ? keisan::clamp(keisan::make_degree(mapped_tilt) + position_min_delta_tilt, position_min_range_tilt, position_max_range_tilt) : position_max_range_tilt; + keisan::Angle min_tilt = (dynamic_kick) ? keisan::make_degree(mapped_tilt) - position_min_range_tilt : position_min_range_tilt; + keisan::Angle max_tilt = (dynamic_kick) ? keisan::make_degree(mapped_tilt) + position_min_range_tilt : position_max_range_tilt; printf("tilt range: %.2f to %.2f\n", min_tilt.degree(), max_tilt.degree()); + printf("pan range: %.2f to %.2f\n", position_min_range_pan, position_max_range_pan); bool tilt_in_range = tilt > min_tilt && tilt < max_tilt; bool right_kick_in_range = pan > position_min_range_pan && pan < -position_center_range_pan; From 154f2c72b70379b61f8c0da80cbee7b88bcbabdb Mon Sep 17 00:00:00 2001 From: Mobilizes Date: Sat, 8 Jun 2024 19:07:45 +0700 Subject: [PATCH 03/10] fix: fix tilt range --- src/suiryoku/locomotion/process/locomotion.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/suiryoku/locomotion/process/locomotion.cpp b/src/suiryoku/locomotion/process/locomotion.cpp index 4b23bda..a508e52 100755 --- a/src/suiryoku/locomotion/process/locomotion.cpp +++ b/src/suiryoku/locomotion/process/locomotion.cpp @@ -671,6 +671,7 @@ bool Locomotion::pivot_inverse_a_move(const keisan::Angle & direction) robot->x_speed = x_speed; robot->y_speed = y_speed; robot->a_speed = a_speed; + robot->aim_on = true; start(); return false; @@ -827,9 +828,10 @@ bool Locomotion::position_kick_range_pan_tilt(const keisan::Angle & dire mapped_tilt = keisan::exponentialmap(pan.degree(), 0.0, position_min_range_pan.degree(), position_min_range_tilt.degree(), position_max_range_tilt.degree()); else if (dynamic_kick && pan >= keisan::make_degree(0.0)) mapped_tilt = keisan::exponentialmap(pan.degree(), 0.0, position_max_range_pan.degree(), position_min_range_tilt.degree(), position_max_range_tilt.degree()); + printf("mapped tilt: %.2f\n", mapped_tilt); - keisan::Angle min_tilt = (dynamic_kick) ? keisan::make_degree(mapped_tilt) - position_min_range_tilt : position_min_range_tilt; - keisan::Angle max_tilt = (dynamic_kick) ? keisan::make_degree(mapped_tilt) + position_min_range_tilt : position_max_range_tilt; + keisan::Angle min_tilt = (dynamic_kick) ? keisan::make_degree(mapped_tilt) - position_min_delta_tilt : position_min_range_tilt; + keisan::Angle max_tilt = (dynamic_kick) ? keisan::make_degree(mapped_tilt) + position_min_delta_tilt : position_max_range_tilt; printf("tilt range: %.2f to %.2f\n", min_tilt.degree(), max_tilt.degree()); printf("pan range: %.2f to %.2f\n", position_min_range_pan, position_max_range_pan); From 50cd023c9011cdd35ecd5b54f3e935ae894f1801 Mon Sep 17 00:00:00 2001 From: hiikariri Date: Thu, 13 Jun 2024 03:08:26 +0700 Subject: [PATCH 04/10] fix: normalize target direction --- src/suiryoku/locomotion/process/locomotion.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/src/suiryoku/locomotion/process/locomotion.cpp b/src/suiryoku/locomotion/process/locomotion.cpp index 9146da5..a93077b 100755 --- a/src/suiryoku/locomotion/process/locomotion.cpp +++ b/src/suiryoku/locomotion/process/locomotion.cpp @@ -285,7 +285,7 @@ bool Locomotion::move_backward_to(const keisan::Point2 & target) return true; } - auto direction = keisan::signed_arctan(delta_y, delta_x) + 180.0_deg; + auto direction = keisan::signed_arctan(delta_y, delta_x).normalize() + 180.0_deg; auto delta_direction = (direction - robot->orientation).normalize().degree(); double x_speed = keisan::map(std::abs(delta_direction), 0.0, 15.0, backward_max_x, backward_min_x); @@ -337,7 +337,7 @@ bool Locomotion::move_forward_to(const keisan::Point2 & target) return true; } - auto direction = keisan::signed_arctan(delta_y, delta_x); + auto direction = keisan::signed_arctan(delta_y, delta_x).normalize(); double delta_direction = (direction - robot->orientation).normalize().degree(); double x_speed = keisan::map(std::abs(delta_direction), 0.0, 15.0, move_max_x, move_min_x); @@ -844,7 +844,7 @@ bool Locomotion::position_kick_range_pan_tilt(const keisan::Angle & dire return true; } - auto target_tilt = 0.5 * (position_min_range_tilt + position_max_range_tilt); + auto target_tilt = right_kick_target_tilt; double delta_tilt = (target_tilt - tilt).degree(); bool pan_in_kick_range = pan > position_min_range_pan && pan < position_max_range_pan; @@ -856,15 +856,16 @@ bool Locomotion::position_kick_range_pan_tilt(const keisan::Angle & dire double x_speed = 0.0; if (!tilt_in_range) { - if (delta_tilt_pan > 3.0) { + if (delta_tilt_pan > 0.0) { x_speed = keisan::map(delta_tilt_pan, 3.0, 20.0, position_min_x * 0.5, position_min_x); - } else if (delta_tilt_pan < -3.0) { + } else if (delta_tilt_pan < 0.0) { x_speed = keisan::map(delta_tilt_pan, -20.0, -3.0, position_max_x, position_max_x * 0.5); } } // a movement double delta_direction = (direction - robot->orientation).normalize().degree(); + std::cerr << "delta direction: " << delta_direction << std::endl; double a_speed = 0; bool direction_in_range = std::fabs(delta_direction) < position_min_delta_direction.degree(); @@ -873,9 +874,9 @@ bool Locomotion::position_kick_range_pan_tilt(const keisan::Angle & dire } // y movement - auto target_pan = pan > 0.0_deg ? 0.5 * (position_center_range_pan + position_max_range_pan) : 0.5 * (-position_center_range_pan + position_min_range_pan); + auto target_pan = pan > 0.0_deg ? left_kick_target_pan : right_kick_target_pan; if (precise_kick) { - target_pan = left_kick ? 0.5 * (position_center_range_pan + position_max_range_pan) : 0.5 * (-position_center_range_pan + position_min_range_pan); + target_pan = left_kick ? left_kick_target_pan : right_kick_target_pan; } double delta_pan = (target_pan - pan).degree(); double y_speed = 0.0; From 9565364c8b02cc425e75b4c2a99ad77dff4299a1 Mon Sep 17 00:00:00 2001 From: hiikariri Date: Thu, 13 Jun 2024 03:16:09 +0700 Subject: [PATCH 05/10] fix: use one algo of inverse a move --- src/suiryoku/locomotion/process/locomotion.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/suiryoku/locomotion/process/locomotion.cpp b/src/suiryoku/locomotion/process/locomotion.cpp index a93077b..cd22fe2 100755 --- a/src/suiryoku/locomotion/process/locomotion.cpp +++ b/src/suiryoku/locomotion/process/locomotion.cpp @@ -671,7 +671,6 @@ bool Locomotion::pivot_inverse_a_move(const keisan::Angle & direction) robot->x_speed = x_speed; robot->y_speed = y_speed; robot->a_speed = a_speed; - robot->aim_on = true; start(); return false; From dbbf0b6887f52d9ee22907076a2afc8de5fadea4 Mon Sep 17 00:00:00 2001 From: hiikariri Date: Sat, 15 Jun 2024 00:38:45 +0700 Subject: [PATCH 06/10] fix: add time filter of positioning to prevent jumping tilt value --- include/suiryoku/locomotion/process/locomotion.hpp | 4 ++-- src/suiryoku/locomotion/process/locomotion.cpp | 11 +++++++++-- 2 files changed, 11 insertions(+), 4 deletions(-) diff --git a/include/suiryoku/locomotion/process/locomotion.hpp b/include/suiryoku/locomotion/process/locomotion.hpp index 46223d3..1bc1210 100755 --- a/include/suiryoku/locomotion/process/locomotion.hpp +++ b/include/suiryoku/locomotion/process/locomotion.hpp @@ -71,7 +71,7 @@ class Locomotion const keisan::Angle & max_pan, const keisan::Angle & min_tilt, const keisan::Angle & max_tilt); bool position_kick_general(const keisan::Angle & direction); - bool position_kick_range_pan_tilt(const keisan::Angle & direction, bool precise_kick, bool left_kick, bool dynamic_kick); + bool position_kick_range_pan_tilt(const keisan::Angle & direction, bool precise_kick, bool left_kick, bool dynamic_kick, double delta_sec); bool is_time_to_follow(); bool pivot_fulfilled(); @@ -89,7 +89,7 @@ class Locomotion bool initial_pivot; private: - + double in_range_sec; double move_min_x; double move_max_x; double move_max_y; diff --git a/src/suiryoku/locomotion/process/locomotion.cpp b/src/suiryoku/locomotion/process/locomotion.cpp index 7a859af..6847080 100755 --- a/src/suiryoku/locomotion/process/locomotion.cpp +++ b/src/suiryoku/locomotion/process/locomotion.cpp @@ -810,7 +810,7 @@ bool Locomotion::position_kick_custom_pan_tilt(const keisan::Angle & dir return false; } -bool Locomotion::position_kick_range_pan_tilt(const keisan::Angle & direction, bool precise_kick, bool left_kick, bool dynamic_kick) +bool Locomotion::position_kick_range_pan_tilt(const keisan::Angle & direction, bool precise_kick, bool left_kick, bool dynamic_kick, double delta_sec) { if (dynamic_kick) { position_max_range_pan = max_dynamic_range_pan; @@ -839,7 +839,14 @@ bool Locomotion::position_kick_range_pan_tilt(const keisan::Angle & dire bool pan_in_range = precise_kick ? (left_kick ? left_kick_in_range : right_kick_in_range) : (right_kick_in_range || left_kick_in_range); if (tilt_in_range && pan_in_range) { - return true; + in_range_sec += delta_sec; + if(in_range_sec >= 0.5) { + in_range_sec = 0.0; + return true; + } + else { + return false; + } } auto target_tilt = right_kick_target_tilt; From 26871eee981f07e4a38a16ac1ba07c70089a4184 Mon Sep 17 00:00:00 2001 From: hiikariri Date: Wed, 19 Jun 2024 23:39:05 +0700 Subject: [PATCH 07/10] feat: refactor positioning, remove time usage --- .../locomotion/process/locomotion.hpp | 3 +- .../locomotion/process/locomotion.cpp | 55 +++++++------------ 2 files changed, 22 insertions(+), 36 deletions(-) diff --git a/include/suiryoku/locomotion/process/locomotion.hpp b/include/suiryoku/locomotion/process/locomotion.hpp index 3ea3e3a..1b9f2db 100755 --- a/include/suiryoku/locomotion/process/locomotion.hpp +++ b/include/suiryoku/locomotion/process/locomotion.hpp @@ -71,7 +71,7 @@ class Locomotion const keisan::Angle & max_pan, const keisan::Angle & min_tilt, const keisan::Angle & max_tilt); bool position_kick_general(const keisan::Angle & direction); - bool position_kick_range_pan_tilt(const keisan::Angle & direction, bool precise_kick, bool left_kick, bool dynamic_kick, double delta_sec); + bool position_kick_range_pan_tilt(const keisan::Angle & direction, bool precise_kick, bool left_kick, bool dynamic_kick); bool is_time_to_follow(); bool pivot_fulfilled(); @@ -89,7 +89,6 @@ class Locomotion bool initial_pivot; private: - double in_range_sec; double move_min_x; double move_max_x; double move_max_y; diff --git a/src/suiryoku/locomotion/process/locomotion.cpp b/src/suiryoku/locomotion/process/locomotion.cpp index 73a2161..220ec91 100755 --- a/src/suiryoku/locomotion/process/locomotion.cpp +++ b/src/suiryoku/locomotion/process/locomotion.cpp @@ -809,7 +809,7 @@ bool Locomotion::position_kick_custom_pan_tilt(const keisan::Angle & dir return false; } -bool Locomotion::position_kick_range_pan_tilt(const keisan::Angle & direction, bool precise_kick, bool left_kick, bool dynamic_kick, double delta_sec) +bool Locomotion::position_kick_range_pan_tilt(const keisan::Angle & direction, bool precise_kick, bool left_kick, bool dynamic_kick) { if (dynamic_kick) { position_max_range_pan = max_dynamic_range_pan; @@ -838,38 +838,41 @@ bool Locomotion::position_kick_range_pan_tilt(const keisan::Angle & dire bool pan_in_range = precise_kick ? (left_kick ? left_kick_in_range : right_kick_in_range) : (right_kick_in_range || left_kick_in_range); if (tilt_in_range && pan_in_range) { - in_range_sec += delta_sec; - if(in_range_sec >= 0.5) { - in_range_sec = 0.0; - return true; - } - else { - return false; - } + return true; } - auto target_tilt = right_kick_target_tilt; + // y movement + left_kick = precise_kick ? left_kick : (pan > 0.0_deg); + auto target_pan = left_kick ? left_kick_target_pan : right_kick_target_pan; + + double delta_pan = (target_pan - pan).degree(); + double y_speed = 0.0; + + if (delta_pan < -position_min_delta_pan.degree()) { + y_speed = keisan::map(delta_pan, -20.0, -position_min_delta_pan.degree(), position_max_ly, position_min_ly); + } else if (delta_pan > position_min_delta_pan.degree()) { + y_speed = keisan::map(delta_pan, position_min_delta_pan.degree(), 20.0, position_min_ry, position_max_ry); + } + + auto target_tilt = left_kick ? left_kick_target_tilt : right_kick_target_tilt; double delta_tilt = (target_tilt - tilt).degree(); bool pan_in_kick_range = pan > position_min_range_pan && pan < position_max_range_pan; - double closest_delta_pan = pan_in_kick_range ? 0 : std::min(std::abs((left_kick_target_pan - pan).degree()), std::abs((right_kick_target_pan - pan).degree())); + double closest_delta_pan = pan_in_kick_range ? 0 : delta_pan; // x movement double delta_tilt_pan = delta_tilt + (closest_delta_pan * 0.3); printf("delta tilt pan %.1f\n", delta_tilt_pan); double x_speed = 0.0; - if (!tilt_in_range) { - if (delta_tilt_pan > 0.0) { - x_speed = keisan::map(delta_tilt_pan, 3.0, 20.0, position_min_x * 0.5, position_min_x); - } else if (delta_tilt_pan < 0.0) { - x_speed = keisan::map(delta_tilt_pan, -20.0, -3.0, position_max_x, position_max_x * 0.5); - } + if (delta_tilt_pan > 3.0) { + x_speed = keisan::map(delta_tilt_pan, 3.0, 20.0, position_min_x * 0.5, position_min_x); + } else if (delta_tilt_pan < -3.0) { + x_speed = keisan::map(delta_tilt_pan, -20.0, -3.0, position_max_x, position_max_x * 0.5); } // a movement double delta_direction = (direction - robot->orientation).normalize().degree(); - std::cerr << "delta direction: " << delta_direction << std::endl; double a_speed = 0; bool direction_in_range = std::fabs(delta_direction) < position_min_delta_direction.degree(); @@ -877,22 +880,6 @@ bool Locomotion::position_kick_range_pan_tilt(const keisan::Angle & dire a_speed = keisan::map(delta_direction, -30.0, 30.0, position_max_a, -position_max_a); } - // y movement - auto target_pan = pan > 0.0_deg ? left_kick_target_pan : right_kick_target_pan; - if (precise_kick) { - target_pan = left_kick ? left_kick_target_pan : right_kick_target_pan; - } - double delta_pan = (target_pan - pan).degree(); - double y_speed = 0.0; - - if (!pan_in_range) { - if (delta_pan < -position_min_delta_pan.degree()) { - y_speed = keisan::map(delta_pan, -20.0, -position_min_delta_pan.degree(), position_max_ly, position_min_ly); - } else if (delta_pan > position_min_delta_pan.degree()) { - y_speed = keisan::map(delta_pan, position_min_delta_pan.degree(), 20.0, position_min_ry, position_max_ry); - } - } - #if ITHAARO || UMARU || MIRU double smooth_ratio = 1.0; #else From 572b5319895c190d8364f907f8b46012a4132684 Mon Sep 17 00:00:00 2001 From: hiikariri Date: Wed, 26 Jun 2024 19:58:30 +0700 Subject: [PATCH 08/10] feat: add dynamic range config --- src/suiryoku/locomotion/process/locomotion.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/src/suiryoku/locomotion/process/locomotion.cpp b/src/suiryoku/locomotion/process/locomotion.cpp index 998a14d..985e408 100755 --- a/src/suiryoku/locomotion/process/locomotion.cpp +++ b/src/suiryoku/locomotion/process/locomotion.cpp @@ -203,6 +203,10 @@ void Locomotion::set_config(const nlohmann::json & json) double position_min_range_pan_double; double position_max_range_pan_double; double position_center_range_pan_double; + double min_dynamic_range_pan_double; + double max_dynamic_range_pan_double; + double min_dynamic_range_tilt_double; + double max_dynamic_range_tilt_double; valid_section &= jitsuyo::assign_val(position_section, "min_x", position_min_x); valid_section &= jitsuyo::assign_val(position_section, "max_x", position_max_x); @@ -221,6 +225,11 @@ void Locomotion::set_config(const nlohmann::json & json) valid_section &= jitsuyo::assign_val(position_section, "max_range_pan", position_max_range_pan_double); valid_section &= jitsuyo::assign_val(position_section, "center_range_pan", position_center_range_pan_double); + valid_section &= jitsuyo::assign_val(position_section, "min_dynamic_range_pan", min_dynamic_range_pan_double); + valid_section &= jitsuyo::assign_val(position_section, "max_dynamic_range_pan", max_dynamic_range_pan_double); + valid_section &= jitsuyo::assign_val(position_section, "min_dynamic_range_tilt", min_dynamic_range_tilt_double); + valid_section &= jitsuyo::assign_val(position_section, "max_dynamic_range_tilt", max_dynamic_range_tilt_double); + position_min_delta_tilt = keisan::make_degree(position_min_delta_tilt_double); position_min_delta_pan = keisan::make_degree(position_min_delta_pan_double); position_min_delta_pan_tilt = keisan::make_degree(position_min_delta_pan_tilt_double); @@ -230,6 +239,10 @@ void Locomotion::set_config(const nlohmann::json & json) position_min_range_pan = keisan::make_degree(position_min_range_pan_double); position_max_range_pan = keisan::make_degree(position_max_range_pan_double); position_center_range_pan = keisan::make_degree(position_center_range_pan_double); + min_dynamic_range_pan = keisan::make_degree(min_dynamic_range_pan_double); + max_dynamic_range_pan = keisan::make_degree(max_dynamic_range_pan_double); + min_dynamic_range_tilt = keisan::make_degree(min_dynamic_range_tilt_double); + max_dynamic_range_tilt = keisan::make_degree(max_dynamic_range_tilt_double); if (!valid_section) { std::cout << "Error found at section `position`" << std::endl; From d2ff8e6043003d35135dc98f31a231590ffcc173 Mon Sep 17 00:00:00 2001 From: hiikariri Date: Sun, 30 Jun 2024 19:29:04 +0700 Subject: [PATCH 09/10] refactor: return to previous style --- src/suiryoku/locomotion/process/locomotion.cpp | 2 +- wget-log | 0 2 files changed, 1 insertion(+), 1 deletion(-) create mode 100644 wget-log diff --git a/src/suiryoku/locomotion/process/locomotion.cpp b/src/suiryoku/locomotion/process/locomotion.cpp index 709b3d9..a4594d2 100755 --- a/src/suiryoku/locomotion/process/locomotion.cpp +++ b/src/suiryoku/locomotion/process/locomotion.cpp @@ -935,7 +935,7 @@ bool Locomotion::position_kick_range_pan_tilt(const keisan::Angle & dire } // y movement - left_kick = precise_kick ? left_kick : (pan > 0.0_deg); + if (!precise_kick) left_kick = pan > 0.0_deg; auto target_pan = left_kick ? left_kick_target_pan : right_kick_target_pan; double delta_pan = (target_pan - pan).degree(); diff --git a/wget-log b/wget-log new file mode 100644 index 0000000..e69de29 From e8a41f665c6552c34a926934677f0439a86af386 Mon Sep 17 00:00:00 2001 From: Nehemy Davis <109798265+hiikariri@users.noreply.github.com> Date: Sun, 30 Jun 2024 19:29:46 +0700 Subject: [PATCH 10/10] Delete wrong wget-log --- wget-log | 0 1 file changed, 0 insertions(+), 0 deletions(-) delete mode 100644 wget-log diff --git a/wget-log b/wget-log deleted file mode 100644 index e69de29..0000000