Skip to content

Commit

Permalink
Merge pull request #31 from ichiro-its/feature/change-pan-target-when…
Browse files Browse the repository at this point in the history 
…-using-90-deg-kick

[Feature] Change the pan target when using 90 deg kick
  • Loading branch information
@hiikariri
hiikariri authored Jul 1, 2024
2 parents d0f7177 + 2bbb53d commit 76a22aa
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 position_kick_range_pan_tilt(const keisan::Angle<double> & direction, bool precise_kick, bool left_kick, bool is_positioning_center);

bool is_time_to_follow();
bool pivot_fulfilled();
Expand Down Expand Up @@ -151,7 +151,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;

valid_section &= jitsuyo::assign_val(position_section, "min_x", position_min_x);
valid_section &= jitsuyo::assign_val(position_section, "max_x", position_max_x);
Expand All @@ -222,7 +223,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);

position_min_delta_tilt = keisan::make_degree(position_min_delta_tilt_double);
position_min_delta_pan = keisan::make_degree(position_min_delta_pan_double);
Expand All @@ -232,7 +234,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);

if (!valid_section) {
std::cout << "Error found at section `position`" << std::endl;
Expand Down Expand Up @@ -887,15 +890,15 @@ 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 Locomotion::position_kick_range_pan_tilt(const keisan::Angle<double> & direction, bool precise_kick, bool left_kick, bool is_positioning_center)
{
auto tilt = robot->get_tilt();
auto pan = robot->get_pan();
auto delta_direction = (direction - robot->orientation).normalize().degree();

bool tilt_in_range = tilt > position_min_range_tilt && tilt < position_max_range_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 @@ -907,6 +910,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 76a22aa

Please sign in to comment.