diff --git a/include/suiryoku/locomotion/process/locomotion.hpp b/include/suiryoku/locomotion/process/locomotion.hpp index 8f5b23c..7c077a7 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, bool is_positioning_center); bool is_time_to_follow(); bool pivot_fulfilled(); @@ -150,7 +150,8 @@ class Locomotion keisan::Angle position_min_range_tilt; keisan::Angle position_max_range_tilt; keisan::Angle position_min_range_pan; - keisan::Angle position_center_range_pan; + keisan::Angle position_center_right_range_pan; + keisan::Angle position_center_left_range_pan; keisan::Angle position_max_range_pan; double skew_max_x; diff --git a/src/suiryoku/locomotion/process/locomotion.cpp b/src/suiryoku/locomotion/process/locomotion.cpp index a4594d2..d84e2b1 100755 --- a/src/suiryoku/locomotion/process/locomotion.cpp +++ b/src/suiryoku/locomotion/process/locomotion.cpp @@ -205,7 +205,8 @@ void Locomotion::set_config(const nlohmann::json & json) double position_max_range_tilt_double; double position_min_range_pan_double; double position_max_range_pan_double; - double position_center_range_pan_double; + double position_center_right_range_pan_double; + double position_center_left_range_pan_double; double min_dynamic_range_pan_double; double max_dynamic_range_pan_double; double min_dynamic_range_tilt_double; @@ -226,7 +227,8 @@ void Locomotion::set_config(const nlohmann::json & json) valid_section &= jitsuyo::assign_val(position_section, "max_range_tilt", position_max_range_tilt_double); valid_section &= jitsuyo::assign_val(position_section, "min_range_pan", position_min_range_pan_double); 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, "center_right_range_pan", position_center_right_range_pan_double); + valid_section &= jitsuyo::assign_val(position_section, "center_left_range_pan", position_center_left_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); @@ -241,7 +243,8 @@ void Locomotion::set_config(const nlohmann::json & json) position_max_range_tilt = keisan::make_degree(position_max_range_tilt_double); 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); + position_center_right_range_pan = keisan::make_degree(position_center_right_range_pan_double); + position_center_left_range_pan = keisan::make_degree(position_center_left_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); @@ -900,7 +903,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, bool is_positioning_center) { if (dynamic_kick) { position_max_range_pan = max_dynamic_range_pan; @@ -925,8 +928,8 @@ bool Locomotion::position_kick_range_pan_tilt(const keisan::Angle & dire printf("pan range: %.2f to %.2f\n", position_min_range_pan.degree(), position_max_range_pan.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 right_kick_in_range = pan > position_min_range_pan && pan < -position_center_right_range_pan; + bool left_kick_in_range = pan > position_center_left_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); bool direction_in_range = std::fabs(delta_direction) < position_min_delta_direction.degree(); @@ -938,6 +941,10 @@ bool Locomotion::position_kick_range_pan_tilt(const keisan::Angle & dire if (!precise_kick) left_kick = pan > 0.0_deg; auto target_pan = left_kick ? left_kick_target_pan : right_kick_target_pan; + if (is_positioning_center) { + target_pan = (left_kick) ? position_center_left_range_pan : -position_center_right_range_pan; + } + double delta_pan = (target_pan - pan).degree(); double y_speed = 0.0;