test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
Walk/OverCome/OverCome.cpp@49:198030e84936, 2019-03-11 (annotated)
- Committer:
- shimizuta
- Date:
- Mon Mar 11 06:36:56 2019 +0000
- Revision:
- 49:198030e84936
- Parent:
- 43:2ed84f3558c1
- Child:
- 50:36741e8ab197
from start to sanddune, 40s (if you use restart)
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 | 43:2ed84f3558c1 | 3 | #include "math.h" |
shimizuta | 35:b4e1b8f25cd7 | 4 | OverCome::OverCome(float start_x_m[4], float start_y_m[4], |
shimizuta | 35:b4e1b8f25cd7 | 5 | float d_x_m, float goal_y_m[4], float height_m[4], float gravity_dist[4], |
shimizuta | 49:198030e84936 | 6 | OneLeg legs[4], float raise_offset_x_m[4]) : walk(legs) |
shimizuta | 35:b4e1b8f25cd7 | 7 | { |
shimizuta | 35:b4e1b8f25cd7 | 8 | for (int i = 0; i < 4; i++) |
shimizuta | 35:b4e1b8f25cd7 | 9 | { |
shimizuta | 35:b4e1b8f25cd7 | 10 | start_x_m_[i] = start_x_m[i]; //足のスタートx |
shimizuta | 35:b4e1b8f25cd7 | 11 | start_y_m_[i] = start_y_m[i]; //足のスタートy |
shimizuta | 35:b4e1b8f25cd7 | 12 | goal_y_m_[i] = goal_y_m[i]; //目標地点までのy |
shimizuta | 35:b4e1b8f25cd7 | 13 | gravity_dist_[i] = gravity_dist[i]; |
shimizuta | 35:b4e1b8f25cd7 | 14 | height_m_[i] = height_m[i]; |
shimizuta | 43:2ed84f3558c1 | 15 | raise_offset_x_m_[i] = raise_offset_x_m[i]; |
shimizuta | 35:b4e1b8f25cd7 | 16 | } |
shimizuta | 35:b4e1b8f25cd7 | 17 | d_x_m_ = d_x_m; //目標地点までのx |
shimizuta | 35:b4e1b8f25cd7 | 18 | |
shimizuta | 49:198030e84936 | 19 | d_time_ = 0.2; //各動きの時間 |
shimizuta | 49:198030e84936 | 20 | d_time_slow_ = 0.3; |
shimizuta | 49:198030e84936 | 21 | |
shimizuta | 35:b4e1b8f25cd7 | 22 | next_point_ = 0; //次のparamのindex |
shimizuta | 35:b4e1b8f25cd7 | 23 | GetLine(); |
shimizuta | 35:b4e1b8f25cd7 | 24 | for (int i = 0; i < 4; i++) |
shimizuta | 35:b4e1b8f25cd7 | 25 | SetOneLegFreeLinesParam(walk, i, legs_[i], next_point_); |
shimizuta | 35:b4e1b8f25cd7 | 26 | } |
shimizuta | 35:b4e1b8f25cd7 | 27 | |
shimizuta | 35:b4e1b8f25cd7 | 28 | void OverCome::Rise(int legnum) |
shimizuta | 35:b4e1b8f25cd7 | 29 | { |
shimizuta | 35:b4e1b8f25cd7 | 30 | for (int i = 0; i < 4; i++) |
shimizuta | 35:b4e1b8f25cd7 | 31 | { |
shimizuta | 35:b4e1b8f25cd7 | 32 | legs_[i][next_point_].time_s = legs_[i][next_point_ - 1].time_s + d_time_; |
shimizuta | 35:b4e1b8f25cd7 | 33 | legs_[i][next_point_].x_m = legs_[i][next_point_ - 1].x_m; |
shimizuta | 35:b4e1b8f25cd7 | 34 | legs_[i][next_point_].y_m = legs_[i][next_point_ - 1].y_m; |
shimizuta | 43:2ed84f3558c1 | 35 | legs_[i][next_point_].is_point_to_point = 0; //直線軌道を維持 |
shimizuta | 35:b4e1b8f25cd7 | 36 | } |
shimizuta | 43:2ed84f3558c1 | 37 | legs_[legnum][next_point_].x_m += raise_offset_x_m_[legnum]; |
shimizuta | 35:b4e1b8f25cd7 | 38 | legs_[legnum][next_point_].y_m -= height_m_[legnum]; |
shimizuta | 35:b4e1b8f25cd7 | 39 | next_point_++; |
shimizuta | 35:b4e1b8f25cd7 | 40 | } |
shimizuta | 35:b4e1b8f25cd7 | 41 | void OverCome::Forward(int legnum) |
shimizuta | 35:b4e1b8f25cd7 | 42 | { |
shimizuta | 35:b4e1b8f25cd7 | 43 | for (int i = 0; i < 4; i++) |
shimizuta | 35:b4e1b8f25cd7 | 44 | { |
shimizuta | 35:b4e1b8f25cd7 | 45 | legs_[i][next_point_].time_s = legs_[i][next_point_ - 1].time_s + d_time_; |
shimizuta | 49:198030e84936 | 46 | legs_[i][next_point_].x_m = legs_[i][next_point_ - 1].x_m; |
shimizuta | 35:b4e1b8f25cd7 | 47 | legs_[i][next_point_].y_m = legs_[i][next_point_ - 1].y_m; |
shimizuta | 43:2ed84f3558c1 | 48 | legs_[i][next_point_].is_point_to_point = 0; //直線軌道を維持 |
shimizuta | 35:b4e1b8f25cd7 | 49 | } |
shimizuta | 43:2ed84f3558c1 | 50 | legs_[legnum][next_point_].x_m += d_x_m_ - raise_offset_x_m_[legnum]; |
shimizuta | 35:b4e1b8f25cd7 | 51 | next_point_++; |
shimizuta | 35:b4e1b8f25cd7 | 52 | } |
shimizuta | 35:b4e1b8f25cd7 | 53 | void OverCome::Land(int legnum) |
shimizuta | 35:b4e1b8f25cd7 | 54 | { |
shimizuta | 35:b4e1b8f25cd7 | 55 | for (int i = 0; i < 4; i++) |
shimizuta | 35:b4e1b8f25cd7 | 56 | { |
shimizuta | 35:b4e1b8f25cd7 | 57 | legs_[i][next_point_].time_s = legs_[i][next_point_ - 1].time_s + d_time_slow_; |
shimizuta | 35:b4e1b8f25cd7 | 58 | legs_[i][next_point_].x_m = legs_[i][next_point_ - 1].x_m; |
shimizuta | 35:b4e1b8f25cd7 | 59 | legs_[i][next_point_].y_m = legs_[i][next_point_ - 1].y_m; |
shimizuta | 43:2ed84f3558c1 | 60 | legs_[i][next_point_].is_point_to_point = 0; //ゆっくり着地するために直線軌道を維持 |
shimizuta | 35:b4e1b8f25cd7 | 61 | } |
shimizuta | 35:b4e1b8f25cd7 | 62 | legs_[legnum][next_point_].y_m = goal_y_m_[legnum]; //着地 |
shimizuta | 35:b4e1b8f25cd7 | 63 | next_point_++; |
shimizuta | 35:b4e1b8f25cd7 | 64 | } |
shimizuta | 35:b4e1b8f25cd7 | 65 | void OverCome::Step(int legnum) |
shimizuta | 35:b4e1b8f25cd7 | 66 | { |
shimizuta | 35:b4e1b8f25cd7 | 67 | Rise(legnum); |
shimizuta | 35:b4e1b8f25cd7 | 68 | Forward(legnum); |
shimizuta | 35:b4e1b8f25cd7 | 69 | Land(legnum); |
shimizuta | 35:b4e1b8f25cd7 | 70 | } |
shimizuta | 35:b4e1b8f25cd7 | 71 | //重心移動、param direct:動く正負。+-1を入れる |
shimizuta | 35:b4e1b8f25cd7 | 72 | void OverCome::GravityMove(int legnum) |
shimizuta | 35:b4e1b8f25cd7 | 73 | { |
shimizuta | 43:2ed84f3558c1 | 74 | if (fabs(gravity_dist_[legnum]) < 0.01) |
shimizuta | 43:2ed84f3558c1 | 75 | return; |
shimizuta | 35:b4e1b8f25cd7 | 76 | for (int i = 0; i < 4; i++) |
shimizuta | 35:b4e1b8f25cd7 | 77 | { |
shimizuta | 35:b4e1b8f25cd7 | 78 | legs_[i][next_point_].time_s = legs_[i][next_point_ - 1].time_s + d_time_; |
shimizuta | 35:b4e1b8f25cd7 | 79 | legs_[i][next_point_].x_m = legs_[i][next_point_ - 1].x_m - gravity_dist_[legnum]; |
shimizuta | 35:b4e1b8f25cd7 | 80 | legs_[i][next_point_].y_m = legs_[i][next_point_ - 1].y_m; |
shimizuta | 43:2ed84f3558c1 | 81 | legs_[i][next_point_].is_point_to_point = 0; //ゆっくり移動するために直線軌道を維持 |
shimizuta | 35:b4e1b8f25cd7 | 82 | } |
shimizuta | 35:b4e1b8f25cd7 | 83 | ++next_point_; |
shimizuta | 35:b4e1b8f25cd7 | 84 | } |
shimizuta | 35:b4e1b8f25cd7 | 85 | void OverCome::StartPoint() |
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 = 0; |
shimizuta | 35:b4e1b8f25cd7 | 90 | legs_[i][next_point_].x_m = start_x_m_[i]; |
shimizuta | 35:b4e1b8f25cd7 | 91 | legs_[i][next_point_].y_m = start_y_m_[i]; |
shimizuta | 35:b4e1b8f25cd7 | 92 | } |
shimizuta | 35:b4e1b8f25cd7 | 93 | ++next_point_; |
shimizuta | 35:b4e1b8f25cd7 | 94 | } |
shimizuta | 35:b4e1b8f25cd7 | 95 | void OverCome::GoalPoint() |
shimizuta | 35:b4e1b8f25cd7 | 96 | { |
shimizuta | 35:b4e1b8f25cd7 | 97 | for (int i = 0; i < 4; i++) |
shimizuta | 35:b4e1b8f25cd7 | 98 | { |
shimizuta | 35:b4e1b8f25cd7 | 99 | legs_[i][next_point_].time_s = legs_[i][next_point_ - 1].time_s + d_time_; |
shimizuta | 35:b4e1b8f25cd7 | 100 | legs_[i][next_point_].x_m = start_x_m_[i]; |
shimizuta | 35:b4e1b8f25cd7 | 101 | legs_[i][next_point_].y_m = goal_y_m_[i]; |
shimizuta | 43:2ed84f3558c1 | 102 | legs_[i][next_point_].is_point_to_point = 0; //ゆっくり移動するために直線軌道を維持 |
shimizuta | 35:b4e1b8f25cd7 | 103 | } |
shimizuta | 35:b4e1b8f25cd7 | 104 | ++next_point_; |
shimizuta | 35:b4e1b8f25cd7 | 105 | } |
shimizuta | 35:b4e1b8f25cd7 | 106 | void OverCome::GetLine() |
shimizuta | 35:b4e1b8f25cd7 | 107 | { |
shimizuta | 35:b4e1b8f25cd7 | 108 | StartPoint(); //スタート時点 |
shimizuta | 35:b4e1b8f25cd7 | 109 | GravityMove(RIGHT_F); |
shimizuta | 35:b4e1b8f25cd7 | 110 | Step(RIGHT_F); |
shimizuta | 35:b4e1b8f25cd7 | 111 | GravityMove(LEFT_B); |
shimizuta | 35:b4e1b8f25cd7 | 112 | Step(LEFT_B); |
shimizuta | 35:b4e1b8f25cd7 | 113 | GravityMove(LEFT_F); |
shimizuta | 35:b4e1b8f25cd7 | 114 | Step(LEFT_F); |
shimizuta | 35:b4e1b8f25cd7 | 115 | GravityMove(RIGHT_B); |
shimizuta | 35:b4e1b8f25cd7 | 116 | Step(RIGHT_B); |
shimizuta | 49:198030e84936 | 117 | // GoalPoint(); |
shimizuta | 35:b4e1b8f25cd7 | 118 | } |