Skip to content

Commit

Permalink
Merge branch 'master' of github.com:ichiro-its/suiryoku into feature/…
Browse files Browse the repository at this point in the history 
…adjust-function-for-dynamic-kick
  • Loading branch information
@hiikariri
hiikariri committed Jul 1, 2024
2 parents e8a41f6 + 76a22aa commit 4ab3c42
Show file tree
Hide file tree
Showing 2 changed files with 16 additions and 8 deletions.
5 changes: 3 additions & 2 deletions include/suiryoku/locomotion/process/locomotion.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -71,7 +71,7 @@ class Locomotion
const keisan::Angle<double> & max_pan, const keisan::Angle<double> & min_tilt,
const keisan::Angle<double> & max_tilt);
bool position_kick_general(const keisan::Angle<double> & direction);
bool position_kick_range_pan_tilt(const keisan::Angle<double> & direction, bool precise_kick, bool left_kick, bool dynamic_kick);
bool position_kick_range_pan_tilt(const keisan::Angle<double> & direction, bool precise_kick, bool left_kick, bool dynamic_kick, bool is_positioning_center);

bool is_time_to_follow();
bool pivot_fulfilled();
Expand Down Expand Up @@ -150,7 +150,8 @@ class Locomotion
keisan::Angle<double> position_min_range_tilt;
keisan::Angle<double> position_max_range_tilt;
keisan::Angle<double> position_min_range_pan;
keisan::Angle<double> position_center_range_pan;
keisan::Angle<double> position_center_right_range_pan;
keisan::Angle<double> position_center_left_range_pan;
keisan::Angle<double> position_max_range_pan;

double skew_max_x;
Expand Down
19 changes: 13 additions & 6 deletions src/suiryoku/locomotion/process/locomotion.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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);
Expand All @@ -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);
Expand Down Expand Up @@ -900,7 +903,7 @@ bool Locomotion::position_kick_custom_pan_tilt(const keisan::Angle<double> & dir
return false;
}

bool Locomotion::position_kick_range_pan_tilt(const keisan::Angle<double> & direction, bool precise_kick, bool left_kick, bool dynamic_kick)
bool Locomotion::position_kick_range_pan_tilt(const keisan::Angle<double> & 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;
Expand All @@ -925,8 +928,8 @@ bool Locomotion::position_kick_range_pan_tilt(const keisan::Angle<double> & 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();

Expand All @@ -938,6 +941,10 @@ bool Locomotion::position_kick_range_pan_tilt(const keisan::Angle<double> & 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;

Expand Down

0 comments on commit 4ab3c42

Please sign in to comment.