春ロボ1班(元F3RC4班+) / PathFollowing1-ver2

春ロボ2019 経路追従プログラム（ver.2）

Dependencies:   CruizCore_R1370P

Fork of PathFollowing1 by 春ロボ1班(元F3RC4班+)

PathFollowing1.h@1:9858517eef98, 2018-10-31 (annotated)

Committer:
yuki0701
Date:
Wed Oct 31 10:13:49 2018 +0000
Revision:
1:9858517eef98
Parent:
0:3d9101dd0061 
a;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yuki0701 0:3d9101dd0061 1 #ifndef HARUROBO2019_PATHFOLLOWING_H
yuki0701 0:3d9101dd0061 2 #define HARUROBO2019_PATHFOLLOWING_H
yuki0701 0:3d9101dd0061 3 #include <EC.h>
yuki0701 0:3d9101dd0061 4 #include <R1370P.h>
yuki0701 0:3d9101dd0061 5
yuki0701 0:3d9101dd0061 6 class pathfollowing1 : public Ec
yuki0701 0:3d9101dd0061 7 {
yuki0701 0:3d9101dd0061 8 private:
yuki0701 0:3d9101dd0061 9 double p_out,r_out;
yuki0701 0:3d9101dd0061 10 double Kvq_p,Kvq_d,Kvr_p,Kvr_d;
yuki0701 0:3d9101dd0061 11 double diff,diff_old,diffangle,diffangle_old;
yuki0701 0:3d9101dd0061 12 double out_dutyQ,out_dutyR;
yuki0701 0:3d9101dd0061 13 double now_angle,target_angle;
yuki0701 0:3d9101dd0061 14 double now_timeQ,old_timeQ,now_timeR,old_timeR;
yuki0701 0:3d9101dd0061 15
yuki0701 0:3d9101dd0061 16 protected:
yuki0701 0:3d9101dd0061 17
yuki0701 0:3d9101dd0061 18 public:
yuki0701 0:3d9101dd0061 19
yuki0701 0:3d9101dd0061 20
yuki0701 0:3d9101dd0061 21
yuki0701 0:3d9101dd0061 22 double now_x, now_y;
yuki0701 0:3d9101dd0061 23
yuki0701 0:3d9101dd0061 24 void XYRmotorout(double plot_x1, double plot_y1, double plot_x2, double plot_y2, double *ad_x_out, double *ad_y_out, double *ad_r_out);
yuki0701 0:3d9101dd0061 25 //出発地点、目標地点の座標から機体のx軸方向、y軸方向、旋回の出力を算出する関数
yuki0701 0:3d9101dd0061 26 /*
yuki0701 0:3d9101dd0061 27 *⒈main分内でx_out,y_out,r_outをdouble型で定義
yuki0701 0:3d9101dd0061 28 *⒉XYRmotorout関数使用時は、num番目のx、y座標、num+1番目のx,y座標に加えx_out,y_out,r_outのアドレス（&x_out,&y_out,&r_out）を渡す→（XYRmotorout(num,&x_out,&y_out,&r_out)）
yuki0701 0:3d9101dd0061 29 */
yuki0701 0:3d9101dd0061 30
yuki0701 0:3d9101dd0061 31
yuki0701 0:3d9101dd0061 32
yuki0701 0:3d9101dd0061 33 void set_p_out(double p);
yuki0701 0:3d9101dd0061 34 //ベクトルABに平行方向の出力値設定関数
yuki0701 0:3d9101dd0061 35
yuki0701 0:3d9101dd0061 36 void q_setPDparam(double q_p,double q_d);
yuki0701 0:3d9101dd0061 37 //ベクトルABに垂直な方向の誤差を埋めるPD制御のパラメータ設定関数
yuki0701 0:3d9101dd0061 38
yuki0701 0:3d9101dd0061 39 void r_setPDparam(double r_p,double r_d);
yuki0701 0:3d9101dd0061 40 //機体角度と目標角度の誤差を埋めるPD制御のパラメータ設定関数
yuki0701 0:3d9101dd0061 41
yuki0701 0:3d9101dd0061 42 void set_r_out(double r);
yuki0701 0:3d9101dd0061 43 //旋回時の最大出力値設定関数
yuki0701 0:3d9101dd0061 44
yuki0701 0:3d9101dd0061 45 void set_target_angle(double t);
yuki0701 0:3d9101dd0061 46 //機体目標角度設定関数
yuki0701 0:3d9101dd0061 47
yuki0701 0:3d9101dd0061 48
yuki0701 0:3d9101dd0061 49 };
yuki0701 0:3d9101dd0061 50 #endif