test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
Diff: Walk/OverCome/OverCome.cpp
- Revision:
- 35:b4e1b8f25cd7
- Child:
- 43:2ed84f3558c1
diff -r 89d701e15cdf -r b4e1b8f25cd7 Walk/OverCome/OverCome.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Walk/OverCome/OverCome.cpp Mon Mar 04 09:54:47 2019 +0000 @@ -0,0 +1,107 @@ +#include "OverCome.h" +#include "change_walk.h" +OverCome::OverCome(float start_x_m[4], float start_y_m[4], + float d_x_m, float goal_y_m[4], float height_m[4], float gravity_dist[4], + OneLeg legs[4]) :walk(legs) +{ + for (int i = 0; i < 4; i++) + { + start_x_m_[i] = start_x_m[i]; //足のスタートx + start_y_m_[i] = start_y_m[i]; //足のスタートy + goal_y_m_[i] = goal_y_m[i]; //目標地点までのy + gravity_dist_[i] = gravity_dist[i]; + height_m_[i] = height_m[i]; + } + d_x_m_ = d_x_m; //目標地点までのx + + d_time_ = 0.5; //各動きの時間 + d_time_slow_ = 0.5; + next_point_ = 0; //次のparamのindex + GetLine(); + for (int i = 0; i < 4; i++) + SetOneLegFreeLinesParam(walk, i, legs_[i], next_point_); +} + +void OverCome::Rise(int legnum) +{ + for (int i = 0; i < 4; i++) + { + legs_[i][next_point_].time_s = legs_[i][next_point_ - 1].time_s + d_time_; + legs_[i][next_point_].x_m = legs_[i][next_point_ - 1].x_m; + legs_[i][next_point_].y_m = legs_[i][next_point_ - 1].y_m; + } + legs_[legnum][next_point_].y_m -= height_m_[legnum]; + next_point_++; +} +void OverCome::Forward(int legnum) +{ + for (int i = 0; i < 4; i++) + { + legs_[i][next_point_].time_s = legs_[i][next_point_ - 1].time_s + d_time_; + legs_[i][next_point_].x_m = legs_[i][next_point_ - 1].x_m; + legs_[i][next_point_].y_m = legs_[i][next_point_ - 1].y_m; + } + legs_[legnum][next_point_].x_m += d_x_m_; + next_point_++; +} +void OverCome::Land(int legnum) +{ + for (int i = 0; i < 4; i++) + { + legs_[i][next_point_].time_s = legs_[i][next_point_ - 1].time_s + d_time_slow_; + legs_[i][next_point_].x_m = legs_[i][next_point_ - 1].x_m; + legs_[i][next_point_].y_m = legs_[i][next_point_ - 1].y_m; + } + legs_[legnum][next_point_].y_m = goal_y_m_[legnum]; //着地 + next_point_++; +} +void OverCome::Step(int legnum) +{ + Rise(legnum); + Forward(legnum); + Land(legnum); +} +//重心移動、param direct:動く正負。+-1を入れる +void OverCome::GravityMove(int legnum) +{ + for (int i = 0; i < 4; i++) + { + legs_[i][next_point_].time_s = legs_[i][next_point_ - 1].time_s + d_time_; + legs_[i][next_point_].x_m = legs_[i][next_point_ - 1].x_m - gravity_dist_[legnum]; + legs_[i][next_point_].y_m = legs_[i][next_point_ - 1].y_m; + } + ++next_point_; +} +void OverCome::StartPoint() +{ + for (int i = 0; i < 4; i++) + { + legs_[i][next_point_].time_s = 0; + legs_[i][next_point_].x_m = start_x_m_[i]; + legs_[i][next_point_].y_m = start_y_m_[i]; + } + ++next_point_; +} +void OverCome::GoalPoint() +{ + for (int i = 0; i < 4; i++) + { + legs_[i][next_point_].time_s = legs_[i][next_point_ - 1].time_s + d_time_; + legs_[i][next_point_].x_m = start_x_m_[i]; + legs_[i][next_point_].y_m = goal_y_m_[i]; + } + ++next_point_; +} +void OverCome::GetLine() +{ + StartPoint(); //スタート時点 + GravityMove(RIGHT_F); + Step(RIGHT_F); + GravityMove(LEFT_B); + Step(LEFT_B); + GravityMove(LEFT_F); + Step(LEFT_F); + GravityMove(RIGHT_B); + Step(RIGHT_B); + GoalPoint(); +} \ No newline at end of file