岡本 陸 / Mbed 2 deprecated 0311

test

Dependencies:   mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM

Walk/Walk.h@34:89d701e15cdf, 2019-03-01 (annotated)

Committer:
shimizuta
Date:
Fri Mar 01 12:07:23 2019 +0000
Revision:
34:89d701e15cdf
Parent:
32:dc684a0b8448
Child:
35:b4e1b8f25cd7
over come and rope

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimizuta 11:e81425872740 1 #ifndef INCLUDED_WALK_H
shimizuta 11:e81425872740 2 #define INCLUDED_WALK_H
shimizuta 11:e81425872740 3 #include "OneLeg.h"
shimizuta 34:89d701e15cdf 4 enum LegNum //足とシリアルサーボの番号
shimizuta 34:89d701e15cdf 5 {
shimizuta 34:89d701e15cdf 6 RIGHT_B,
shimizuta 34:89d701e15cdf 7 RIGHT_F,
shimizuta 34:89d701e15cdf 8 LEFT_B,
shimizuta 34:89d701e15cdf 9 LEFT_F,
shimizuta 34:89d701e15cdf 10 };
shimizuta 32:dc684a0b8448 11 enum TriangleParams
shimizuta 27:79b4b932a6dd 12 {
shimizuta 32:dc684a0b8448 13 OFFSET_X_M,
shimizuta 32:dc684a0b8448 14 OFFSET_Y_M,
shimizuta 32:dc684a0b8448 15 STRIDE_M,
shimizuta 32:dc684a0b8448 16 HEIGHT_M,
shimizuta 32:dc684a0b8448 17 BUFFER_HEIGHT_M,
shimizuta 27:79b4b932a6dd 18 };
shimizuta 29:7d8b8011a88d 19 //下記の論文を参照
shimizuta 29:7d8b8011a88d 20 //https://static1.squarespace.com/static/542ddec8e4b0158794bd1036/t/5a861d1cc83025f3d460dfc3/1518738728029/gait-design-optimization-Final.pdf
shimizuta 29:7d8b8011a88d 21 class TriangleOrbit
shimizuta 27:79b4b932a6dd 22 {
shimizuta 29:7d8b8011a88d 23 float reverse_tanbeta_; //論文のβのtan
shimizuta 32:dc684a0b8448 24 float offset_x_m_;
shimizuta 32:dc684a0b8448 25 float offset_y_m_;
shimizuta 29:7d8b8011a88d 26 float stride_m_;
shimizuta 29:7d8b8011a88d 27 float height_m_; //足上げ幅
shimizuta 29:7d8b8011a88d 28 float buffer_height_m_; //着地直前で止める高さ
shimizuta 29:7d8b8011a88d 29 float stridetime_s_;
shimizuta 29:7d8b8011a88d 30 float toptime_s_; //頂点に行くまでの時間
shimizuta 29:7d8b8011a88d 31 float buffer_time_s_; //一時停止点から着地するまでの時間.
shimizuta 29:7d8b8011a88d 32
shimizuta 29:7d8b8011a88d 33 float top_x_m_, top_y_m_, buffer_x_m_, buffer_y_m_; //事前に計算して保存しておく
shimizuta 29:7d8b8011a88d 34 int StrideLine_(OneLeg &leg, float phasetime_s); //着地している間の軌道
shimizuta 29:7d8b8011a88d 35 int StrideLineAccel_(OneLeg &leg, float phasetime_s); //着地中に慣性力を考慮して加減速
shimizuta 11:e81425872740 36
shimizuta 29:7d8b8011a88d 37 public:
shimizuta 34:89d701e15cdf 38 void CalOtherParam();
shimizuta 32:dc684a0b8448 39 void SetTriangleParam(float offset_x_m, float offset_y_m, float stride_m, float height_m, float buffer_height_m,
shimizuta 29:7d8b8011a88d 40 float stridetime_s, float toptime_s, float buffer_time_s);
shimizuta 29:7d8b8011a88d 41 int GetOrbit(OneLeg &leg, float phasetime_s);
shimizuta 29:7d8b8011a88d 42 float GetOneWalkTime(); //足一周の時間
shimizuta 32:dc684a0b8448 43 void ChangeOneParam(TriangleParams param, float val);
shimizuta 29:7d8b8011a88d 44 };
shimizuta 29:7d8b8011a88d 45
shimizuta 32:dc684a0b8448 46 struct LineParam
shimizuta 32:dc684a0b8448 47 {
shimizuta 34:89d701e15cdf 48 float time_s; //x_m,y_mにいる時間
shimizuta 32:dc684a0b8448 49 float x_m;
shimizuta 32:dc684a0b8448 50 float y_m;
shimizuta 32:dc684a0b8448 51 };
shimizuta 32:dc684a0b8448 52 class FreeLineOrbit
shimizuta 32:dc684a0b8448 53 {
shimizuta 34:89d701e15cdf 54 LineParam lineparams_[30]; //pointの数は多めにとっている。
shimizuta 32:dc684a0b8448 55 int point_num_; //pointの数
shimizuta 32:dc684a0b8448 56 public:
shimizuta 32:dc684a0b8448 57 void SetFreeLinesParam(LineParam lineparams[], int point_num); //任意の直線の軌道を設定
shimizuta 32:dc684a0b8448 58 int GetOrbit(OneLeg &leg, float phasetime_s);
shimizuta 34:89d701e15cdf 59 float GetOneWalkTime(); //足一周の時間
shimizuta 34:89d701e15cdf 60 void SetStandParam(float x_m, float y_m, float time_s); //Standupのparam設定
shimizuta 32:dc684a0b8448 61 };
shimizuta 32:dc684a0b8448 62 enum OrbitPattern
shimizuta 32:dc684a0b8448 63 {
shimizuta 32:dc684a0b8448 64 TRIANGLE,
shimizuta 32:dc684a0b8448 65 FREELINES,
shimizuta 32:dc684a0b8448 66 };
shimizuta 29:7d8b8011a88d 67 //足の軌道を設定するクラスの皮。全ての軌道クラスを継承して一つにまとめる。patternで使い分ける
shimizuta 32:dc684a0b8448 68 //関数は継承元のものも使えるので、継承元のクラスも見てください
shimizuta 32:dc684a0b8448 69 class Orbit : public TriangleOrbit, public FreeLineOrbit
shimizuta 29:7d8b8011a88d 70 {
shimizuta 29:7d8b8011a88d 71 OrbitPattern pattern_;
shimizuta 29:7d8b8011a88d 72
shimizuta 29:7d8b8011a88d 73 public:
shimizuta 29:7d8b8011a88d 74 Orbit(){}; //ただ宣言する用
shimizuta 29:7d8b8011a88d 75 Orbit(OrbitPattern pattern);
shimizuta 29:7d8b8011a88d 76 float GetOneWalkTime(); //足一周の時間
shimizuta 29:7d8b8011a88d 77 int GetOrbit(OneLeg &leg, float phasetime_s); //legに目標x,yを代入
shimizuta 27:79b4b932a6dd 78 };
shimizuta 11:e81425872740 79 class Walk
shimizuta 11:e81425872740 80 {
shimizuta 34:89d701e15cdf 81 Orbit orbit_[4];
shimizuta 29:7d8b8011a88d 82 float offset_multi_[4];
shimizuta 11:e81425872740 83
shimizuta 29:7d8b8011a88d 84 public:
shimizuta 29:7d8b8011a88d 85 Walk();
shimizuta 29:7d8b8011a88d 86 static float calctime_s_; //計算周期
shimizuta 29:7d8b8011a88d 87 float phasetime_s_[4];
shimizuta 29:7d8b8011a88d 88 //参照渡しされたonelegそれぞれにorbit_::GetOrbit関数を代入
shimizuta 29:7d8b8011a88d 89 //GetOrbit関数にて軌道, 時間をもとに位置を決める
shimizuta 29:7d8b8011a88d 90 //直接参照しているのはこの関数だけ
shimizuta 29:7d8b8011a88d 91 int Cal4LegsPosi(OneLeg leg[4]); //失敗したら1を返す。成功なら0
shimizuta 29:7d8b8011a88d 92 //足一周の時間
shimizuta 29:7d8b8011a88d 93 float GetOneWalkTime();
shimizuta 29:7d8b8011a88d 94 //位相ずれの程度(値域[0,1])を入れる。
shimizuta 29:7d8b8011a88d 95 void SetOffsetTime(float offset_multi0, float offset_multi1, float offset_multi2, float offset_multi3);
shimizuta 29:7d8b8011a88d 96 //軌道がリンク定義外になっていないかチェック。reutn 0:ok 1:out
shimizuta 29:7d8b8011a88d 97 // またoffsetの設定はここで行うので必ず呼び出す。
shimizuta 29:7d8b8011a88d 98 int CheckOrbit(OneLeg templateleg);
shimizuta 29:7d8b8011a88d 99 //一つの足の軌道を設定.軌道の代入にはこの関数を必ず通るようにする。
shimizuta 29:7d8b8011a88d 100 void SetOneOrbit(int legnum, Orbit orbit);
shimizuta 29:7d8b8011a88d 101 void SetAllOrbit(Orbit orbit); //全足の軌道を一括で設定.
shimizuta 29:7d8b8011a88d 102 //stand用
shimizuta 29:7d8b8011a88d 103 void SetOneLegStandParam(int legnum, float x_m, float y_m, float time_s);
shimizuta 29:7d8b8011a88d 104 void SetAllLegStandParam(float x_m, float y_m, float time_s); // 4足一括で設定できる
shimizuta 29:7d8b8011a88d 105 //triangle
shimizuta 32:dc684a0b8448 106 void SetOneLegTriangleParam(int legnum, float offset_x_m, float offset_y_m, float stride_m, float height_m, float buffer_height_m,
shimizuta 32:dc684a0b8448 107 float stridetime_s, float toptime_s, float buffer_time_s);
shimizuta 32:dc684a0b8448 108 void SetAllLegTriangleParam(float offset_x_m, float offset_y_m, float stride_m, float height_m, float buffer_height_m,
shimizuta 29:7d8b8011a88d 109 float stridetime_s, float toptime_s, float buffer_time_s);
shimizuta 32:dc684a0b8448 110 void ChangeOneParam(int legnum, TriangleParams param, float val);
shimizuta 32:dc684a0b8448 111 //freeline
shimizuta 32:dc684a0b8448 112 void SetOneLegFreeLinesParam(int legnum, LineParam lineparams[], int point_num);
shimizuta 32:dc684a0b8448 113 void SetAllLegFreeLinesParam(LineParam lineparams[], int point_num);
shimizuta 11:e81425872740 114 };
shimizuta 32:dc684a0b8448 115
shimizuta 34:89d701e15cdf 116 class OverCome
shimizuta 34:89d701e15cdf 117 {
shimizuta 34:89d701e15cdf 118 float start_x_m_[4]; //足のスタートx
shimizuta 34:89d701e15cdf 119 float start_y_m_[4]; //足のスタートy
shimizuta 34:89d701e15cdf 120 float d_x_m_; //目標地点までのx
shimizuta 34:89d701e15cdf 121 float goal_y_m_[4]; //目標地点までのy
shimizuta 34:89d701e15cdf 122 float height_m_[4];
shimizuta 34:89d701e15cdf 123 float gravity_dist_[4]; //重心ずらし距離
shimizuta 34:89d701e15cdf 124 float d_time_; //各動きの時間
shimizuta 34:89d701e15cdf 125 float d_time_slow_;
shimizuta 34:89d701e15cdf 126 int next_point_; //次のparamのindex
shimizuta 34:89d701e15cdf 127 LineParam legs_[4][30]; //多めに
shimizuta 34:89d701e15cdf 128 void Rise(int legnum);
shimizuta 34:89d701e15cdf 129 void Land(int legnum);
shimizuta 34:89d701e15cdf 130 void Forward(int legnum);
shimizuta 34:89d701e15cdf 131 void Step(int legnum);
shimizuta 34:89d701e15cdf 132 void GravityMove(int legnum);
shimizuta 34:89d701e15cdf 133 void StartPoint();
shimizuta 34:89d701e15cdf 134 void GoalPoint();
shimizuta 34:89d701e15cdf 135 void GetLine();
shimizuta 34:89d701e15cdf 136
shimizuta 34:89d701e15cdf 137 public:
shimizuta 34:89d701e15cdf 138 Walk walk;
shimizuta 34:89d701e15cdf 139 OverCome(float start_x_m[4], float start_y_m[4],
shimizuta 34:89d701e15cdf 140 float d_x_m, float goal_y_m[4], float height_m[4], float gravity_dist[4]);
shimizuta 34:89d701e15cdf 141 };
shimizuta 11:e81425872740 142 #endif