test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
Diff: Walk/Walk.h
- Revision:
- 27:79b4b932a6dd
- Parent:
- 22:787a764271dd
- Child:
- 29:7d8b8011a88d
--- a/Walk/Walk.h Fri Feb 22 05:20:28 2019 +0000 +++ b/Walk/Walk.h Wed Feb 27 01:24:00 2019 +0000 @@ -1,50 +1,99 @@ #ifndef INCLUDED_WALK_H #define INCLUDED_WALK_H #include "OneLeg.h" -//足の軌道を設定するクラス。軌道は様々であることを考慮してコンストラクタでの代入は控えたい -class Orbit +enum EllipseParams { - float stridetime_s_; - float risetime_s_; + STRIDE_M, + HEIGHT_M, + GROUND_M, + ELLIPSE_CENTER_X_M, + ELLIPSE_CENTER_Y_M, +}; +class EllipseOrbit +{ float stride_m_; - float height_m_; //足の上げ幅 - float ellipse_center_y_m_; //楕円軌道の中心高さ - float ellipse_center_x_m_; //楕円軌道の中心x座標 - float ground_m_; //地面の高さ - float ellipse_long_m_; //楕円の長径 - float ellipse_short_m_; //楕円の短径 - - OneLeg StrideLine_(OneLeg leg, float phasetime_s); //時間をもとに位置を取得したインスタンスを直接返して代入させる - OneLeg RiseEllipse_(OneLeg leg, float phasetime_s); - OneLeg OrbitEllipse_(OneLeg leg, float phasetime_s); //Stride, Riseを組み合わせて周期を作る - OneLeg RiseEllipse2_(OneLeg leg, float phasetime_s); + float height_m_; //足の上げ幅 + float ellipsecenter_y_m_; //楕円軌道の中心高さ + float ellipsecenter_x_m_; //楕円軌道の中心x座標 + float ground_m_; //地面の高さ + int StrideLine_(OneLeg &leg, float phasetime_s); //着地している間の軌道 + int StrideLineAccel_(OneLeg &leg, float phasetime_s); //着地中に慣性力を考慮して加減速 + int RiseEllipse_(OneLeg &leg, float phasetime_s); //空中を楕円軌道の半分の軌道で動く + int RiseEllipse2_(OneLeg &leg, float phasetime_s); //空中を楕円軌道の一部欠けの形で動く public: - Orbit(){}; - //このクラスのメイン privateへのアクセス関数 - OneLeg GetOrbit(OneLeg leg, float phasetime_s); - void SetStraightParam(float stridetime_s, float risetime_s, - float stride_m, float height_m, float ground_m, - float ellipse_center_x_m, float ellipse_center_y_m); - void SetStandParam(float ground_m); + int GetOrbit(OneLeg &leg, float phasetime_s); + //paramセット用。普段はこれを使う + 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); + // ひとつのparamだけ変更 + void ChangeOneParam(EllipseParams param, float val); float GetOneWalkTime(); //足一周の時間 + float stridetime_s_; + float risetime_s_; }; +struct LineParam +{ + float time_s; //次の点に到達するまでにかける時間 + float x_m; + float y_m; +}; +class FreeLineOrbit +{ + LineParam lineparams_[10]; //pointの数はmax10としている。多めにとっているだけ。 + int point_num_; //pointの数 + public: + void SetFreeLinesParam(LineParam lineparams[], int point_num); //任意の直線の軌道を設定 + int GetOrbit(OneLeg &leg, float phasetime_s); + float GetOneWalkTime(); //足一周の時間 + //Standupのparam設定 + void SetStandParam(float x_m, float y_m, float time_s); +}; +enum OrbitPattern +{ + ELLIPSE, + FREELINES, +}; +//足の軌道を設定するクラスの皮。全ての軌道クラスを継承して一つにまとめる。patternで使い分ける +class Orbit : public EllipseOrbit, public FreeLineOrbit +{ + OrbitPattern pattern_; -//4足の足の動かし方を保存するクラス + public: + Orbit(){}; //ただ宣言する用 + Orbit(OrbitPattern pattern); + float GetOneWalkTime(); //足一周の時間 + int GetOrbit(OneLeg &leg, float phasetime_s); //legに目標x,yを代入 +}; class Walk { - float phasetime_s_; - float offsettime_s_[4]; + Orbit orbit_[4]; + float offset_multi_[4]; public: - //4脚の軌道, 位相を代入して歩行方法を確定する - //コンストラクタではメンバの代入のみ行っている - Walk(Orbit orbit[4], float offsettime_s[4], float cycletime_s); - float cycletime_s_; //計算周期 - Orbit orbit_[4]; + Walk(); + static float calctime_s_; //計算周期 + float phasetime_s_[4]; //参照渡しされたonelegそれぞれにorbit_::GetOrbit関数を代入 //GetOrbit関数にて軌道, 時間をもとに位置を決める //直接参照しているのはこの関数だけ - void Cal4LegsPosi(OneLeg (&leg)[4]); + void Cal4LegsPosi(OneLeg leg[4]); + //足一周の時間 + float GetOneWalkTime(); + //位相ずれの程度(値域[0,1])を入れる。 + void SetOffset(float offset_multi0, float offset_multi1, float offset_multi2, float offset_multi3); + //軌道がリンク定義外になっていないかチェック。reutn 0:ok 1:out + // またoffsetの設定はここで行うので必ず呼び出す。 + int CheckOrbit(OneLeg templateleg); + //一つの足の軌道を設定.軌道の代入にはこの関数を必ず通るようにする。 + void SetOneOrbit(int legnum, Orbit orbit); + //全足の軌道を一括で設定. + void SetAllOrbit(Orbit orbit); + void ChangeOneParam(int legnum, EllipseParams param, float val); + //standのparamを4足一括で設定できる + void SetAllLegStandParam(float x_m, float y_m, float time_s); + //ellipseのparamを4足一括で設定できる + void SetAllLegEllipseParam(float stridetime_s, float risetime_s, + float stride_m, float height_m, float ground_m, + float ellipsecenter_x_m, float ellipsecenter_y_m); }; #endif \ No newline at end of file