岡本 陸 / Mbed 2 deprecated 0311

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?

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