diff --git a/include/suiryoku/locomotion/model/robot.hpp b/include/suiryoku/locomotion/model/robot.hpp index 86c4517..4b62520 100644 --- a/include/suiryoku/locomotion/model/robot.hpp +++ b/include/suiryoku/locomotion/model/robot.hpp @@ -57,6 +57,7 @@ class Robot double y_speed; double a_speed; bool aim_on; + bool inverse_a_move; }; } // namespace suiryoku diff --git a/include/suiryoku/locomotion/process/locomotion.hpp b/include/suiryoku/locomotion/process/locomotion.hpp index 2c3e7a8..b46c24f 100755 --- a/include/suiryoku/locomotion/process/locomotion.hpp +++ b/include/suiryoku/locomotion/process/locomotion.hpp @@ -59,6 +59,7 @@ class Locomotion bool dribble(const keisan::Angle & direction); bool pivot(const keisan::Angle & direction); + bool pivot_inverse_a_move(const keisan::Angle & direction); bool position_until( const keisan::Angle & target_pan, @@ -85,6 +86,8 @@ class Locomotion std::string config_name; + bool initial_pivot; + private: double move_min_x; diff --git a/src/suiryoku/locomotion/node/locomotion_node.cpp b/src/suiryoku/locomotion/node/locomotion_node.cpp index ad21df9..a4e8793 100644 --- a/src/suiryoku/locomotion/node/locomotion_node.cpp +++ b/src/suiryoku/locomotion/node/locomotion_node.cpp @@ -95,6 +95,7 @@ void LocomotionNode::publish_walking() walking_msg.y_move = robot->y_speed; walking_msg.a_move = robot->a_speed; walking_msg.aim_on = robot->aim_on; + walking_msg.inverse_a_move = robot->inverse_a_move; set_walking_publisher->publish(walking_msg); } diff --git a/src/suiryoku/locomotion/process/locomotion.cpp b/src/suiryoku/locomotion/process/locomotion.cpp index 18a140f..4f3dace 100755 --- a/src/suiryoku/locomotion/process/locomotion.cpp +++ b/src/suiryoku/locomotion/process/locomotion.cpp @@ -616,6 +616,62 @@ bool Locomotion::pivot(const keisan::Angle & direction) return false; } +bool Locomotion::pivot_inverse_a_move(const keisan::Angle & direction) +{ + if (initial_pivot) { + initial_pivot = false; + walk_in_position(); + return false; + } + + auto delta_direction = (direction - robot->orientation).normalize().degree(); + + if (std::abs(delta_direction) < pivot_max_delta_direction) { + walk_in_position(); + robot->inverse_a_move = false; + return true; + } + + double delta_tilt = (pivot_target_tilt - robot->tilt + robot->tilt_center).degree(); + + printf("pivot inverse a move\n"); + printf("delta direction %f\n", delta_direction); + printf("delta tilt %f\n", delta_tilt); + + // x_movement + double x_speed = delta_tilt > 0.0 + ? keisan::map(delta_tilt, 0.0, 20.0, 0.0, pivot_min_x) + : keisan::map(delta_tilt, -20.0, 0.0, pivot_max_x, 0.0); + + // y movement + double y_speed = delta_direction < 0 + ? keisan::map(delta_direction, 180.0, 0.0, pivot_max_ry * 0.9, pivot_max_ry) + : keisan::map(delta_direction, -180.0, 0.0, pivot_max_ly * 0.9, pivot_max_ly); + + // a movement + double a_speed = y_speed < 0.0 + ? keisan::map(delta_direction, -180.0, 0.0, -pivot_max_a * 0.9, -pivot_max_a) + : keisan::map(delta_direction, 180.0, 0.0, pivot_max_a, pivot_max_a * 0.9); + + #if ITHAARO || UMARU || MIRU + double smooth_ratio = 1.0; + #else + double smooth_ratio = 0.8; + #endif + + a_speed = keisan::smooth(robot->a_speed, a_speed, smooth_ratio); + x_speed = keisan::smooth(robot->x_speed, x_speed, smooth_ratio); + y_speed = keisan::smooth(robot->y_speed, y_speed, smooth_ratio); + + robot->inverse_a_move = true; + robot->x_speed = x_speed; + robot->y_speed = y_speed; + robot->a_speed = a_speed; + start(); + + return false; +} + bool Locomotion::position_until( const keisan::Angle & target_pan, const keisan::Angle & target_tilt,