岡本 陸 / Mbed 2 deprecated 0311

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?

UserRevisionLine numberNew 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 }