Kobayashi Akihiro / ActiveCaster

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependents:   ActiveCaster_ ActiveCaster_2

PathTracking.h@2:f206311600ee, 2022-01-28 (annotated)

Committer:
e5119053f6
Date:
Fri Jan 28 15:43:18 2022 +0000
Revision:
2:f206311600ee
Parent:
0:5e4f1e288e2a 
DDSS

Who changed what in which revision?

UserRevisionLine numberNew contents of line
e5119053f6 0:5e4f1e288e2a 1 #ifndef PATHTRACKING_h
e5119053f6 0:5e4f1e288e2a 2 #define PATHTRACKING_h
e5119053f6 0:5e4f1e288e2a 3
e5119053f6 0:5e4f1e288e2a 4 #include "mbed.h"
e5119053f6 0:5e4f1e288e2a 5 #include "PIDclass.h"
e5119053f6 0:5e4f1e288e2a 6 #include "Filter.h"
e5119053f6 0:5e4f1e288e2a 7 #include "define.h"
e5119053f6 0:5e4f1e288e2a 8
e5119053f6 0:5e4f1e288e2a 9 #define PATHNUM 50
e5119053f6 0:5e4f1e288e2a 10 #define POINTNUM 100
e5119053f6 0:5e4f1e288e2a 11
e5119053f6 0:5e4f1e288e2a 12 #define FOLLOW_TANGENT ( 0 )
e5119053f6 0:5e4f1e288e2a 13 #define FOLLOW_COMMAND ( 1 )
e5119053f6 0:5e4f1e288e2a 14 //#define FOLLOW_ICHIPID ( 2 )
e5119053f6 0:5e4f1e288e2a 15 #define POSITION_PID ( 2 )
e5119053f6 0:5e4f1e288e2a 16
e5119053f6 0:5e4f1e288e2a 17 #define MODE_START ( 0 )
e5119053f6 0:5e4f1e288e2a 18 #define MODE_NORMAL ( 1 )
e5119053f6 0:5e4f1e288e2a 19 #define MODE_STOP ( 2 )
e5119053f6 0:5e4f1e288e2a 20 #define MODE_START_STOP ( 3 )
e5119053f6 0:5e4f1e288e2a 21
e5119053f6 0:5e4f1e288e2a 22 class PathTracking{
e5119053f6 0:5e4f1e288e2a 23 public:
e5119053f6 0:5e4f1e288e2a 24 /*********** 変数宣言 ***********/
e5119053f6 0:5e4f1e288e2a 25 double Px[POINTNUM], Py[POINTNUM];
e5119053f6 0:5e4f1e288e2a 26 double refangle[PATHNUM], refvel[PATHNUM];
e5119053f6 0:5e4f1e288e2a 27 int acc_mode[PATHNUM], acc_count[PATHNUM];
e5119053f6 0:5e4f1e288e2a 28 double dec_tbe[PATHNUM];
e5119053f6 0:5e4f1e288e2a 29
e5119053f6 0:5e4f1e288e2a 30 // ベジエ曲線関連
e5119053f6 0:5e4f1e288e2a 31 double Ax[ PATHNUM ];
e5119053f6 0:5e4f1e288e2a 32 double Bx[ PATHNUM ];
e5119053f6 0:5e4f1e288e2a 33 double Cx[ PATHNUM ];
e5119053f6 0:5e4f1e288e2a 34 double Dx[ PATHNUM ];
e5119053f6 0:5e4f1e288e2a 35
e5119053f6 0:5e4f1e288e2a 36 double Ay[ PATHNUM ];
e5119053f6 0:5e4f1e288e2a 37 double By[ PATHNUM ];
e5119053f6 0:5e4f1e288e2a 38 double Cy[ PATHNUM ];
e5119053f6 0:5e4f1e288e2a 39 double Dy[ PATHNUM ];
e5119053f6 0:5e4f1e288e2a 40
e5119053f6 0:5e4f1e288e2a 41 // 内積関連
e5119053f6 0:5e4f1e288e2a 42 double a_be[ PATHNUM ];
e5119053f6 0:5e4f1e288e2a 43 double b_be[ PATHNUM ];
e5119053f6 0:5e4f1e288e2a 44 double c_be[ PATHNUM ];
e5119053f6 0:5e4f1e288e2a 45 double d_be[ PATHNUM ];
e5119053f6 0:5e4f1e288e2a 46 double e_be[ PATHNUM ];
e5119053f6 0:5e4f1e288e2a 47 double f_be[ PATHNUM ];
e5119053f6 0:5e4f1e288e2a 48 double d_be_[ PATHNUM ];
e5119053f6 0:5e4f1e288e2a 49 double e_be_[ PATHNUM ];
e5119053f6 0:5e4f1e288e2a 50 double f_be_[ PATHNUM ];
e5119053f6 0:5e4f1e288e2a 51
e5119053f6 0:5e4f1e288e2a 52 double onx, ony;
e5119053f6 0:5e4f1e288e2a 53 double angle, dist;
e5119053f6 0:5e4f1e288e2a 54 double preAngle = 1.5708;
e5119053f6 0:5e4f1e288e2a 55 double t_be, pre_t_be;
e5119053f6 0:5e4f1e288e2a 56 double dist2goal;
e5119053f6 0:5e4f1e288e2a 57 double epsilon;
e5119053f6 0:5e4f1e288e2a 58
e5119053f6 0:5e4f1e288e2a 59 double refVx, refVy, refVz;
e5119053f6 0:5e4f1e288e2a 60 double refKakudo;
e5119053f6 0:5e4f1e288e2a 61 double tmpPx, tmpPy;
e5119053f6 0:5e4f1e288e2a 62
e5119053f6 0:5e4f1e288e2a 63 /*********** 関数宣言 ***********/
e5119053f6 0:5e4f1e288e2a 64 PathTracking(int xmode);
e5119053f6 0:5e4f1e288e2a 65 // tを求めるための方程式
e5119053f6 0:5e4f1e288e2a 66 double func(int p, double t);
e5119053f6 0:5e4f1e288e2a 67 double dfunc(int p, double t);
e5119053f6 0:5e4f1e288e2a 68 // tにおけるベジエ曲線の座標を求める関数
e5119053f6 0:5e4f1e288e2a 69 double bezier_x(int p, double t);
e5119053f6 0:5e4f1e288e2a 70 double bezier_y(int p, double t);
e5119053f6 0:5e4f1e288e2a 71 // ベジエ曲線式の1階微分
e5119053f6 0:5e4f1e288e2a 72 double dbezier_x(int p, double t);
e5119053f6 0:5e4f1e288e2a 73 double dbezier_y(int p, double t);
e5119053f6 0:5e4f1e288e2a 74
e5119053f6 0:5e4f1e288e2a 75 void initSettings();
e5119053f6 0:5e4f1e288e2a 76
e5119053f6 0:5e4f1e288e2a 77 void calcRefpoint();
e5119053f6 0:5e4f1e288e2a 78 int calcRefvel();
e5119053f6 0:5e4f1e288e2a 79
e5119053f6 0:5e4f1e288e2a 80 void incrPathnum(double conv_length, double conv_tnum);
e5119053f6 0:5e4f1e288e2a 81 void setConvPara(double conv_length, double conv_tnum);
e5119053f6 0:5e4f1e288e2a 82 int getPathNum();
e5119053f6 0:5e4f1e288e2a 83 void setPathNum(int);
e5119053f6 0:5e4f1e288e2a 84
e5119053f6 0:5e4f1e288e2a 85 void setMode(int);
e5119053f6 0:5e4f1e288e2a 86 int getMode();
e5119053f6 0:5e4f1e288e2a 87
e5119053f6 0:5e4f1e288e2a 88 void setMaxPathnum(int);
e5119053f6 0:5e4f1e288e2a 89
e5119053f6 0:5e4f1e288e2a 90 void setPosiPIDxPara(float xKp, float xKi, float xKd);
e5119053f6 0:5e4f1e288e2a 91 void setPosiPIDyPara(float xKp, float xKi, float xKd);
e5119053f6 0:5e4f1e288e2a 92 void setPosiPIDzPara(float xKp, float xKi, float xKd);
e5119053f6 0:5e4f1e288e2a 93 void setYokozurePIDPara(float xKp, float xKi, float xKd);
e5119053f6 0:5e4f1e288e2a 94 void setKakudoPIDPara(float xKp, float xKi, float xKd);
e5119053f6 0:5e4f1e288e2a 95 void kakudoPIDinit();
e5119053f6 0:5e4f1e288e2a 96 void setRefKakudo();
e5119053f6 0:5e4f1e288e2a 97
e5119053f6 0:5e4f1e288e2a 98 double getRefVper();
e5119053f6 0:5e4f1e288e2a 99 double getRefVrot();
e5119053f6 0:5e4f1e288e2a 100
e5119053f6 0:5e4f1e288e2a 101 private:
e5119053f6 0:5e4f1e288e2a 102 int path_num;
e5119053f6 0:5e4f1e288e2a 103 int mode;
e5119053f6 0:5e4f1e288e2a 104 int max_pathnum;
e5119053f6 0:5e4f1e288e2a 105 int count_acc;
e5119053f6 0:5e4f1e288e2a 106
e5119053f6 0:5e4f1e288e2a 107 double conv_length;
e5119053f6 0:5e4f1e288e2a 108 double conv_tnum;
e5119053f6 0:5e4f1e288e2a 109
e5119053f6 0:5e4f1e288e2a 110 bool mode_changed;
e5119053f6 0:5e4f1e288e2a 111 bool init_done;
e5119053f6 0:5e4f1e288e2a 112
e5119053f6 0:5e4f1e288e2a 113 double tan, per, rot;
e5119053f6 0:5e4f1e288e2a 114 };
e5119053f6 0:5e4f1e288e2a 115
e5119053f6 0:5e4f1e288e2a 116 #endif