test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
Walk/OverCome/OverCome.cpp@35:b4e1b8f25cd7, 2019-03-04 (annotated)
- Committer:
- shimizuta
- Date:
- Mon Mar 04 09:54:47 2019 +0000
- Revision:
- 35:b4e1b8f25cd7
- Child:
- 43:2ed84f3558c1
new MR2;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
shimizuta | 35:b4e1b8f25cd7 | 1 | #include "OverCome.h" |
shimizuta | 35:b4e1b8f25cd7 | 2 | #include "change_walk.h" |
shimizuta | 35:b4e1b8f25cd7 | 3 | OverCome::OverCome(float start_x_m[4], float start_y_m[4], |
shimizuta | 35:b4e1b8f25cd7 | 4 | float d_x_m, float goal_y_m[4], float height_m[4], float gravity_dist[4], |
shimizuta | 35:b4e1b8f25cd7 | 5 | OneLeg legs[4]) :walk(legs) |
shimizuta | 35:b4e1b8f25cd7 | 6 | { |
shimizuta | 35:b4e1b8f25cd7 | 7 | for (int i = 0; i < 4; i++) |
shimizuta | 35:b4e1b8f25cd7 | 8 | { |
shimizuta | 35:b4e1b8f25cd7 | 9 | start_x_m_[i] = start_x_m[i]; //足のスタートx |
shimizuta | 35:b4e1b8f25cd7 | 10 | start_y_m_[i] = start_y_m[i]; //足のスタートy |
shimizuta | 35:b4e1b8f25cd7 | 11 | goal_y_m_[i] = goal_y_m[i]; //目標地点までのy |
shimizuta | 35:b4e1b8f25cd7 | 12 | gravity_dist_[i] = gravity_dist[i]; |
shimizuta | 35:b4e1b8f25cd7 | 13 | height_m_[i] = height_m[i]; |
shimizuta | 35:b4e1b8f25cd7 | 14 | } |
shimizuta | 35:b4e1b8f25cd7 | 15 | d_x_m_ = d_x_m; //目標地点までのx |
shimizuta | 35:b4e1b8f25cd7 | 16 | |
shimizuta | 35:b4e1b8f25cd7 | 17 | d_time_ = 0.5; //各動きの時間 |
shimizuta | 35:b4e1b8f25cd7 | 18 | d_time_slow_ = 0.5; |
shimizuta | 35:b4e1b8f25cd7 | 19 | next_point_ = 0; //次のparamのindex |
shimizuta | 35:b4e1b8f25cd7 | 20 | GetLine(); |
shimizuta | 35:b4e1b8f25cd7 | 21 | for (int i = 0; i < 4; i++) |
shimizuta | 35:b4e1b8f25cd7 | 22 | SetOneLegFreeLinesParam(walk, i, legs_[i], next_point_); |
shimizuta | 35:b4e1b8f25cd7 | 23 | } |
shimizuta | 35:b4e1b8f25cd7 | 24 | |
shimizuta | 35:b4e1b8f25cd7 | 25 | void OverCome::Rise(int legnum) |
shimizuta | 35:b4e1b8f25cd7 | 26 | { |
shimizuta | 35:b4e1b8f25cd7 | 27 | for (int i = 0; i < 4; i++) |
shimizuta | 35:b4e1b8f25cd7 | 28 | { |
shimizuta | 35:b4e1b8f25cd7 | 29 | legs_[i][next_point_].time_s = legs_[i][next_point_ - 1].time_s + d_time_; |
shimizuta | 35:b4e1b8f25cd7 | 30 | legs_[i][next_point_].x_m = legs_[i][next_point_ - 1].x_m; |
shimizuta | 35:b4e1b8f25cd7 | 31 | legs_[i][next_point_].y_m = legs_[i][next_point_ - 1].y_m; |
shimizuta | 35:b4e1b8f25cd7 | 32 | } |
shimizuta | 35:b4e1b8f25cd7 | 33 | legs_[legnum][next_point_].y_m -= height_m_[legnum]; |
shimizuta | 35:b4e1b8f25cd7 | 34 | next_point_++; |
shimizuta | 35:b4e1b8f25cd7 | 35 | } |
shimizuta | 35:b4e1b8f25cd7 | 36 | void OverCome::Forward(int legnum) |
shimizuta | 35:b4e1b8f25cd7 | 37 | { |
shimizuta | 35:b4e1b8f25cd7 | 38 | for (int i = 0; i < 4; i++) |
shimizuta | 35:b4e1b8f25cd7 | 39 | { |
shimizuta | 35:b4e1b8f25cd7 | 40 | legs_[i][next_point_].time_s = legs_[i][next_point_ - 1].time_s + d_time_; |
shimizuta | 35:b4e1b8f25cd7 | 41 | legs_[i][next_point_].x_m = legs_[i][next_point_ - 1].x_m; |
shimizuta | 35:b4e1b8f25cd7 | 42 | legs_[i][next_point_].y_m = legs_[i][next_point_ - 1].y_m; |
shimizuta | 35:b4e1b8f25cd7 | 43 | } |
shimizuta | 35:b4e1b8f25cd7 | 44 | legs_[legnum][next_point_].x_m += d_x_m_; |
shimizuta | 35:b4e1b8f25cd7 | 45 | next_point_++; |
shimizuta | 35:b4e1b8f25cd7 | 46 | } |
shimizuta | 35:b4e1b8f25cd7 | 47 | void OverCome::Land(int legnum) |
shimizuta | 35:b4e1b8f25cd7 | 48 | { |
shimizuta | 35:b4e1b8f25cd7 | 49 | for (int i = 0; i < 4; i++) |
shimizuta | 35:b4e1b8f25cd7 | 50 | { |
shimizuta | 35:b4e1b8f25cd7 | 51 | legs_[i][next_point_].time_s = legs_[i][next_point_ - 1].time_s + d_time_slow_; |
shimizuta | 35:b4e1b8f25cd7 | 52 | legs_[i][next_point_].x_m = legs_[i][next_point_ - 1].x_m; |
shimizuta | 35:b4e1b8f25cd7 | 53 | legs_[i][next_point_].y_m = legs_[i][next_point_ - 1].y_m; |
shimizuta | 35:b4e1b8f25cd7 | 54 | } |
shimizuta | 35:b4e1b8f25cd7 | 55 | legs_[legnum][next_point_].y_m = goal_y_m_[legnum]; //着地 |
shimizuta | 35:b4e1b8f25cd7 | 56 | next_point_++; |
shimizuta | 35:b4e1b8f25cd7 | 57 | } |
shimizuta | 35:b4e1b8f25cd7 | 58 | void OverCome::Step(int legnum) |
shimizuta | 35:b4e1b8f25cd7 | 59 | { |
shimizuta | 35:b4e1b8f25cd7 | 60 | Rise(legnum); |
shimizuta | 35:b4e1b8f25cd7 | 61 | Forward(legnum); |
shimizuta | 35:b4e1b8f25cd7 | 62 | Land(legnum); |
shimizuta | 35:b4e1b8f25cd7 | 63 | } |
shimizuta | 35:b4e1b8f25cd7 | 64 | //重心移動、param direct:動く正負。+-1を入れる |
shimizuta | 35:b4e1b8f25cd7 | 65 | void OverCome::GravityMove(int legnum) |
shimizuta | 35:b4e1b8f25cd7 | 66 | { |
shimizuta | 35:b4e1b8f25cd7 | 67 | for (int i = 0; i < 4; i++) |
shimizuta | 35:b4e1b8f25cd7 | 68 | { |
shimizuta | 35:b4e1b8f25cd7 | 69 | legs_[i][next_point_].time_s = legs_[i][next_point_ - 1].time_s + d_time_; |
shimizuta | 35:b4e1b8f25cd7 | 70 | legs_[i][next_point_].x_m = legs_[i][next_point_ - 1].x_m - gravity_dist_[legnum]; |
shimizuta | 35:b4e1b8f25cd7 | 71 | legs_[i][next_point_].y_m = legs_[i][next_point_ - 1].y_m; |
shimizuta | 35:b4e1b8f25cd7 | 72 | } |
shimizuta | 35:b4e1b8f25cd7 | 73 | ++next_point_; |
shimizuta | 35:b4e1b8f25cd7 | 74 | } |
shimizuta | 35:b4e1b8f25cd7 | 75 | void OverCome::StartPoint() |
shimizuta | 35:b4e1b8f25cd7 | 76 | { |
shimizuta | 35:b4e1b8f25cd7 | 77 | for (int i = 0; i < 4; i++) |
shimizuta | 35:b4e1b8f25cd7 | 78 | { |
shimizuta | 35:b4e1b8f25cd7 | 79 | legs_[i][next_point_].time_s = 0; |
shimizuta | 35:b4e1b8f25cd7 | 80 | legs_[i][next_point_].x_m = start_x_m_[i]; |
shimizuta | 35:b4e1b8f25cd7 | 81 | legs_[i][next_point_].y_m = start_y_m_[i]; |
shimizuta | 35:b4e1b8f25cd7 | 82 | } |
shimizuta | 35:b4e1b8f25cd7 | 83 | ++next_point_; |
shimizuta | 35:b4e1b8f25cd7 | 84 | } |
shimizuta | 35:b4e1b8f25cd7 | 85 | void OverCome::GoalPoint() |
shimizuta | 35:b4e1b8f25cd7 | 86 | { |
shimizuta | 35:b4e1b8f25cd7 | 87 | for (int i = 0; i < 4; i++) |
shimizuta | 35:b4e1b8f25cd7 | 88 | { |
shimizuta | 35:b4e1b8f25cd7 | 89 | legs_[i][next_point_].time_s = legs_[i][next_point_ - 1].time_s + d_time_; |
shimizuta | 35:b4e1b8f25cd7 | 90 | legs_[i][next_point_].x_m = start_x_m_[i]; |
shimizuta | 35:b4e1b8f25cd7 | 91 | legs_[i][next_point_].y_m = goal_y_m_[i]; |
shimizuta | 35:b4e1b8f25cd7 | 92 | } |
shimizuta | 35:b4e1b8f25cd7 | 93 | ++next_point_; |
shimizuta | 35:b4e1b8f25cd7 | 94 | } |
shimizuta | 35:b4e1b8f25cd7 | 95 | void OverCome::GetLine() |
shimizuta | 35:b4e1b8f25cd7 | 96 | { |
shimizuta | 35:b4e1b8f25cd7 | 97 | StartPoint(); //スタート時点 |
shimizuta | 35:b4e1b8f25cd7 | 98 | GravityMove(RIGHT_F); |
shimizuta | 35:b4e1b8f25cd7 | 99 | Step(RIGHT_F); |
shimizuta | 35:b4e1b8f25cd7 | 100 | GravityMove(LEFT_B); |
shimizuta | 35:b4e1b8f25cd7 | 101 | Step(LEFT_B); |
shimizuta | 35:b4e1b8f25cd7 | 102 | GravityMove(LEFT_F); |
shimizuta | 35:b4e1b8f25cd7 | 103 | Step(LEFT_F); |
shimizuta | 35:b4e1b8f25cd7 | 104 | GravityMove(RIGHT_B); |
shimizuta | 35:b4e1b8f25cd7 | 105 | Step(RIGHT_B); |
shimizuta | 35:b4e1b8f25cd7 | 106 | GoalPoint(); |
shimizuta | 35:b4e1b8f25cd7 | 107 | } |