岡本 陸 / Mbed 2 deprecated 0311

test

Dependencies:   mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM

Walk/Walk.h@27:79b4b932a6dd, 2019-02-27 (annotated)

Committer:
shimizuta
Date:
Wed Feb 27 01:24:00 2019 +0000
Revision:
27:79b4b932a6dd
Parent:
22:787a764271dd
Child:
29:7d8b8011a88d
debug was ok. ROS is not debuged

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 27:79b4b932a6dd 4 enum EllipseParams
shimizuta 11:e81425872740 5 {
shimizuta 27:79b4b932a6dd 6 STRIDE_M,
shimizuta 27:79b4b932a6dd 7 HEIGHT_M,
shimizuta 27:79b4b932a6dd 8 GROUND_M,
shimizuta 27:79b4b932a6dd 9 ELLIPSE_CENTER_X_M,
shimizuta 27:79b4b932a6dd 10 ELLIPSE_CENTER_Y_M,
shimizuta 27:79b4b932a6dd 11 };
shimizuta 27:79b4b932a6dd 12 class EllipseOrbit
shimizuta 27:79b4b932a6dd 13 {
shimizuta 11:e81425872740 14 float stride_m_;
shimizuta 27:79b4b932a6dd 15 float height_m_; //足の上げ幅
shimizuta 27:79b4b932a6dd 16 float ellipsecenter_y_m_; //楕円軌道の中心高さ
shimizuta 27:79b4b932a6dd 17 float ellipsecenter_x_m_; //楕円軌道の中心x座標
shimizuta 27:79b4b932a6dd 18 float ground_m_; //地面の高さ
shimizuta 27:79b4b932a6dd 19 int StrideLine_(OneLeg &leg, float phasetime_s); //着地している間の軌道
shimizuta 27:79b4b932a6dd 20 int StrideLineAccel_(OneLeg &leg, float phasetime_s); //着地中に慣性力を考慮して加減速
shimizuta 27:79b4b932a6dd 21 int RiseEllipse_(OneLeg &leg, float phasetime_s); //空中を楕円軌道の半分の軌道で動く
shimizuta 27:79b4b932a6dd 22 int RiseEllipse2_(OneLeg &leg, float phasetime_s); //空中を楕円軌道の一部欠けの形で動く
shimizuta 11:e81425872740 23
shimizuta 11:e81425872740 24 public:
shimizuta 27:79b4b932a6dd 25 int GetOrbit(OneLeg &leg, float phasetime_s);
shimizuta 27:79b4b932a6dd 26 //paramセット用。普段はこれを使う
shimizuta 27:79b4b932a6dd 27 void SetParam(float stridetime_s, float risetime_s, float stride_m, float height_m, float ground_m, float ellipsecenter_x_m, float ellipsecenter_y_m);
shimizuta 27:79b4b932a6dd 28 // ひとつのparamだけ変更
shimizuta 27:79b4b932a6dd 29 void ChangeOneParam(EllipseParams param, float val);
shimizuta 11:e81425872740 30 float GetOneWalkTime(); //足一周の時間
shimizuta 27:79b4b932a6dd 31 float stridetime_s_;
shimizuta 27:79b4b932a6dd 32 float risetime_s_;
shimizuta 11:e81425872740 33 };
shimizuta 27:79b4b932a6dd 34 struct LineParam
shimizuta 27:79b4b932a6dd 35 {
shimizuta 27:79b4b932a6dd 36 float time_s; //次の点に到達するまでにかける時間
shimizuta 27:79b4b932a6dd 37 float x_m;
shimizuta 27:79b4b932a6dd 38 float y_m;
shimizuta 27:79b4b932a6dd 39 };
shimizuta 27:79b4b932a6dd 40 class FreeLineOrbit
shimizuta 27:79b4b932a6dd 41 {
shimizuta 27:79b4b932a6dd 42 LineParam lineparams_[10]; //pointの数はmax10としている。多めにとっているだけ。
shimizuta 27:79b4b932a6dd 43 int point_num_; //pointの数
shimizuta 27:79b4b932a6dd 44 public:
shimizuta 27:79b4b932a6dd 45 void SetFreeLinesParam(LineParam lineparams[], int point_num); //任意の直線の軌道を設定
shimizuta 27:79b4b932a6dd 46 int GetOrbit(OneLeg &leg, float phasetime_s);
shimizuta 27:79b4b932a6dd 47 float GetOneWalkTime(); //足一周の時間
shimizuta 27:79b4b932a6dd 48 //Standupのparam設定
shimizuta 27:79b4b932a6dd 49 void SetStandParam(float x_m, float y_m, float time_s);
shimizuta 27:79b4b932a6dd 50 };
shimizuta 27:79b4b932a6dd 51 enum OrbitPattern
shimizuta 27:79b4b932a6dd 52 {
shimizuta 27:79b4b932a6dd 53 ELLIPSE,
shimizuta 27:79b4b932a6dd 54 FREELINES,
shimizuta 27:79b4b932a6dd 55 };
shimizuta 27:79b4b932a6dd 56 //足の軌道を設定するクラスの皮。全ての軌道クラスを継承して一つにまとめる。patternで使い分ける
shimizuta 27:79b4b932a6dd 57 class Orbit : public EllipseOrbit, public FreeLineOrbit
shimizuta 27:79b4b932a6dd 58 {
shimizuta 27:79b4b932a6dd 59 OrbitPattern pattern_;
shimizuta 11:e81425872740 60
shimizuta 27:79b4b932a6dd 61 public:
shimizuta 27:79b4b932a6dd 62 Orbit(){}; //ただ宣言する用
shimizuta 27:79b4b932a6dd 63 Orbit(OrbitPattern pattern);
shimizuta 27:79b4b932a6dd 64 float GetOneWalkTime(); //足一周の時間
shimizuta 27:79b4b932a6dd 65 int GetOrbit(OneLeg &leg, float phasetime_s); //legに目標x,yを代入
shimizuta 27:79b4b932a6dd 66 };
shimizuta 11:e81425872740 67 class Walk
shimizuta 11:e81425872740 68 {
shimizuta 27:79b4b932a6dd 69 Orbit orbit_[4];
shimizuta 27:79b4b932a6dd 70 float offset_multi_[4];
shimizuta 11:e81425872740 71
shimizuta 11:e81425872740 72 public:
shimizuta 27:79b4b932a6dd 73 Walk();
shimizuta 27:79b4b932a6dd 74 static float calctime_s_; //計算周期
shimizuta 27:79b4b932a6dd 75 float phasetime_s_[4];
yuto17320508 13:e7ecdb20665a 76 //参照渡しされたonelegそれぞれにorbit_::GetOrbit関数を代入
yuto17320508 13:e7ecdb20665a 77 //GetOrbit関数にて軌道, 時間をもとに位置を決める
yuto17320508 13:e7ecdb20665a 78 //直接参照しているのはこの関数だけ
shimizuta 27:79b4b932a6dd 79 void Cal4LegsPosi(OneLeg leg[4]);
shimizuta 27:79b4b932a6dd 80 //足一周の時間
shimizuta 27:79b4b932a6dd 81 float GetOneWalkTime();
shimizuta 27:79b4b932a6dd 82 //位相ずれの程度(値域[0,1])を入れる。
shimizuta 27:79b4b932a6dd 83 void SetOffset(float offset_multi0, float offset_multi1, float offset_multi2, float offset_multi3);
shimizuta 27:79b4b932a6dd 84 //軌道がリンク定義外になっていないかチェック。reutn 0:ok 1:out
shimizuta 27:79b4b932a6dd 85 // またoffsetの設定はここで行うので必ず呼び出す。
shimizuta 27:79b4b932a6dd 86 int CheckOrbit(OneLeg templateleg);
shimizuta 27:79b4b932a6dd 87 //一つの足の軌道を設定.軌道の代入にはこの関数を必ず通るようにする。
shimizuta 27:79b4b932a6dd 88 void SetOneOrbit(int legnum, Orbit orbit);
shimizuta 27:79b4b932a6dd 89 //全足の軌道を一括で設定.
shimizuta 27:79b4b932a6dd 90 void SetAllOrbit(Orbit orbit);
shimizuta 27:79b4b932a6dd 91 void ChangeOneParam(int legnum, EllipseParams param, float val);
shimizuta 27:79b4b932a6dd 92 //standのparamを4足一括で設定できる
shimizuta 27:79b4b932a6dd 93 void SetAllLegStandParam(float x_m, float y_m, float time_s);
shimizuta 27:79b4b932a6dd 94 //ellipseのparamを4足一括で設定できる
shimizuta 27:79b4b932a6dd 95 void SetAllLegEllipseParam(float stridetime_s, float risetime_s,
shimizuta 27:79b4b932a6dd 96 float stride_m, float height_m, float ground_m,
shimizuta 27:79b4b932a6dd 97 float ellipsecenter_x_m, float ellipsecenter_y_m);
shimizuta 11:e81425872740 98 };
shimizuta 11:e81425872740 99 #endif