PathTracking.h

00001 #ifndef PATHTRACKING_h
00002 #define PATHTRACKING_h
00003 
00004 #include "mbed.h"
00005 #include "PIDclass.h"
00006 #include "Filter.h"
00007 #include "define.h"
00008 
00009 #define PATHNUM 50
00010 #define POINTNUM 100
00011 
00012 #define FOLLOW_TANGENT  ( 0 )
00013 #define FOLLOW_COMMAND  ( 1 )
00014 //#define FOLLOW_ICHIPID  ( 2 )
00015 #define POSITION_PID    ( 2 )
00016 
00017 #define MODE_START  ( 0 )
00018 #define MODE_NORMAL ( 1 )
00019 #define MODE_STOP   ( 2 )
00020 #define MODE_START_STOP   ( 3 )
00021 
00022 class PathTracking{
00023 public:
00024     /*********** 変数宣言 ***********/
00025     double Px[POINTNUM], Py[POINTNUM];
00026     double refangle[PATHNUM], refvel[PATHNUM];
00027     int acc_mode[PATHNUM], acc_count[PATHNUM];
00028     double dec_tbe[PATHNUM];
00029 
00030     // ベジエ曲線関連
00031     double Ax[ PATHNUM ];
00032     double Bx[ PATHNUM ];
00033     double Cx[ PATHNUM ];
00034     double Dx[ PATHNUM ];
00035 
00036     double Ay[ PATHNUM ];
00037     double By[ PATHNUM ];
00038     double Cy[ PATHNUM ];
00039     double Dy[ PATHNUM ];
00040 
00041     // 内積関連
00042     double a_be[ PATHNUM ];
00043     double b_be[ PATHNUM ];
00044     double c_be[ PATHNUM ];
00045     double d_be[ PATHNUM ];
00046     double e_be[ PATHNUM ];
00047     double f_be[ PATHNUM ];
00048     double d_be_[ PATHNUM ];
00049     double e_be_[ PATHNUM ];
00050     double f_be_[ PATHNUM ];
00051 
00052     double onx, ony;
00053     double angle, dist;
00054     double preAngle = 1.5708;
00055     double t_be, pre_t_be;
00056     double dist2goal;
00057     double epsilon;
00058 
00059     double refVx, refVy, refVz;
00060     double refKakudo;
00061     double tmpPx, tmpPy;
00062 
00063     /*********** 関数宣言 ***********/
00064     PathTracking(int xmode);
00065     // tを求めるための方程式
00066     double func(int p, double t);
00067     double dfunc(int p, double t);
00068     // tにおけるベジエ曲線の座標を求める関数
00069     double bezier_x(int p, double t);
00070     double bezier_y(int p, double t);
00071     // ベジエ曲線式の1階微分
00072     double dbezier_x(int p, double t);
00073     double dbezier_y(int p, double t);
00074 
00075     void initSettings();
00076 
00077     void calcRefpoint();
00078     int calcRefvel();
00079     
00080     void incrPathnum(double conv_length, double conv_tnum);
00081     void setConvPara(double conv_length, double conv_tnum);
00082     int getPathNum();
00083     void setPathNum(int);
00084 
00085     void setMode(int);
00086     int getMode();
00087 
00088     void setMaxPathnum(int);
00089 
00090     void setPosiPIDxPara(float xKp, float xKi, float xKd);
00091     void setPosiPIDyPara(float xKp, float xKi, float xKd);
00092     void setPosiPIDzPara(float xKp, float xKi, float xKd);
00093     void setYokozurePIDPara(float xKp, float xKi, float xKd);
00094     void setKakudoPIDPara(float xKp, float xKi, float xKd);
00095     void kakudoPIDinit();
00096     void setRefKakudo();
00097 
00098     double getRefVper();
00099     double getRefVrot();
00100 
00101 private:
00102     int path_num;
00103     int mode;
00104     int max_pathnum;
00105     int count_acc;
00106 
00107     double conv_length;
00108     double conv_tnum;
00109 
00110     bool mode_changed;
00111     bool init_done;
00112 
00113     double tan, per, rot;
00114 };
00115 
00116 #endif