File content as of revision 34:89d701e15cdf:

#ifndef INCLUDED_WALK_H
#define INCLUDED_WALK_H
#include "OneLeg.h"
enum LegNum //足とシリアルサーボの番号
{
  RIGHT_B,
  RIGHT_F,
  LEFT_B,
  LEFT_F,
};
enum TriangleParams
{
  OFFSET_X_M,
  OFFSET_Y_M,
  STRIDE_M,
  HEIGHT_M,
  BUFFER_HEIGHT_M,
};
//下記の論文を参照
//https://static1.squarespace.com/static/542ddec8e4b0158794bd1036/t/5a861d1cc83025f3d460dfc3/1518738728029/gait-design-optimization-Final.pdf
class TriangleOrbit
{
  float reverse_tanbeta_; //論文のβのtan
  float offset_x_m_;
  float offset_y_m_;
  float stride_m_;
  float height_m_;        //足上げ幅
  float buffer_height_m_; //着地直前で止める高さ
  float stridetime_s_;
  float toptime_s_;     //頂点に行くまでの時間
  float buffer_time_s_; //一時停止点から着地するまでの時間.

  float top_x_m_, top_y_m_, buffer_x_m_, buffer_y_m_;   //事前に計算して保存しておく
  int StrideLine_(OneLeg &leg, float phasetime_s);      //着地している間の軌道
  int StrideLineAccel_(OneLeg &leg, float phasetime_s); //着地中に慣性力を考慮して加減速

public:
void CalOtherParam();
  void SetTriangleParam(float offset_x_m, float offset_y_m, float stride_m, float height_m, float buffer_height_m,
                        float stridetime_s, float toptime_s, float buffer_time_s);
  int GetOrbit(OneLeg &leg, float phasetime_s);
  float GetOneWalkTime(); //足一周の時間
  void ChangeOneParam(TriangleParams param, float val);
};

struct LineParam
{
  float time_s; //x_m,y_mにいる時間
  float x_m;
  float y_m;
};
class FreeLineOrbit
{
  LineParam lineparams_[30]; //pointの数は多めにとっている。
  int point_num_;            //pointの数
public:
  void SetFreeLinesParam(LineParam lineparams[], int point_num); //任意の直線の軌道を設定
  int GetOrbit(OneLeg &leg, float phasetime_s);
  float GetOneWalkTime();                                 //足一周の時間
  void SetStandParam(float x_m, float y_m, float time_s); //Standupのparam設定
};
enum OrbitPattern
{
  TRIANGLE,
  FREELINES,
};
//足の軌道を設定するクラスの皮。全ての軌道クラスを継承して一つにまとめる。patternで使い分ける
//関数は継承元のものも使えるので、継承元のクラスも見てください
class Orbit : public TriangleOrbit, public FreeLineOrbit
{
  OrbitPattern pattern_;

public:
  Orbit(){}; //ただ宣言する用
  Orbit(OrbitPattern pattern);
  float GetOneWalkTime();                       //足一周の時間
  int GetOrbit(OneLeg &leg, float phasetime_s); //legに目標x,yを代入
};
class Walk
{
  Orbit orbit_[4];
  float offset_multi_[4];

public:
  Walk();
  static float calctime_s_; //計算周期
  float phasetime_s_[4];
  //参照渡しされたonelegそれぞれにorbit_::GetOrbit関数を代入
  //GetOrbit関数にて軌道, 時間をもとに位置を決める
  //直接参照しているのはこの関数だけ
  int Cal4LegsPosi(OneLeg leg[4]); //失敗したら1を返す。成功なら0
  //足一周の時間
  float GetOneWalkTime();
  //位相ずれの程度(値域[0,1])を入れる。
  void SetOffsetTime(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); //全足の軌道を一括で設定.
  //stand用
  void SetOneLegStandParam(int legnum, float x_m, float y_m, float time_s);
  void SetAllLegStandParam(float x_m, float y_m, float time_s); // 4足一括で設定できる
  //triangle
  void SetOneLegTriangleParam(int legnum, float offset_x_m, float offset_y_m, float stride_m, float height_m, float buffer_height_m,
                              float stridetime_s, float toptime_s, float buffer_time_s);
  void SetAllLegTriangleParam(float offset_x_m, float offset_y_m, float stride_m, float height_m, float buffer_height_m,
                              float stridetime_s, float toptime_s, float buffer_time_s);
  void ChangeOneParam(int legnum, TriangleParams param, float val);
  //freeline
  void SetOneLegFreeLinesParam(int legnum, LineParam lineparams[], int point_num);
  void SetAllLegFreeLinesParam(LineParam lineparams[], int point_num);
};

class OverCome
{
  float start_x_m_[4]; //足のスタートx
  float start_y_m_[4]; //足のスタートy
  float d_x_m_;        //目標地点までのx
  float goal_y_m_[4];  //目標地点までのy
  float height_m_[4];
  float gravity_dist_[4]; //重心ずらし距離
  float d_time_;          //各動きの時間
  float d_time_slow_;
  int next_point_;        //次のparamのindex
  LineParam legs_[4][30]; //多めに
  void Rise(int legnum);
  void Land(int legnum);
  void Forward(int legnum);
  void Step(int legnum);
  void GravityMove(int legnum);
  void StartPoint();
  void GoalPoint();
  void GetLine();

public:
  Walk walk;
  OverCome(float start_x_m[4], float start_y_m[4],
           float d_x_m, float goal_y_m[4], float height_m[4], float gravity_dist[4]);
};
#endif