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?
User | Revision | Line number | New 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 |