春ロボ1班(元F3RC4班+) / PathFollowing_12_22

harurobo_mbed_undercarriage_sub

PathFollowing.cpp@2:32362343f091, 2018-12-01 (annotated)

Committer:
yuki0701
Date:
Sat Dec 01 05:17:07 2018 +0000
Revision:
2:32362343f091
Parent:
1:d438093cb464
Child:
3:5da150ef209c
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
la00noix 0:591749f315ac 1 #include <PathFollowing.h>
la00noix 0:591749f315ac 2 #include <mbed.h>
la00noix 0:591749f315ac 3 #include <math.h>
la00noix 0:591749f315ac 4
yuki0701 1:d438093cb464 5 double p_out,r_out_max;
la00noix 0:591749f315ac 6 double Kvq_p,Kvq_d,Kvr_p,Kvr_d;
la00noix 0:591749f315ac 7 double diff_old,diffangle,diffangle_old;
la00noix 0:591749f315ac 8 double out_dutyQ,out_dutyR;
la00noix 0:591749f315ac 9 double now_angle,target_angle;
la00noix 0:591749f315ac 10 double now_timeQ,old_timeQ,now_timeR,old_timeR;
la00noix 0:591749f315ac 11 double now_x, now_y;
yuki0701 2:32362343f091 12 double diff_tgt, p_param;
la00noix 0:591749f315ac 13
yuki0701 1:d438093cb464 14
la00noix 0:591749f315ac 15 Timer timer;
la00noix 0:591749f315ac 16
la00noix 0:591749f315ac 17 //初期座標：A, 目標座標：B、機体位置：C、点Cから直線ABに下ろした垂線の足：H
yuki0701 2:32362343f091 18 void XYRmotorout(int type, double plot_x1, double plot_y1, double plot_x2, double plot_y2, double *ad_x_out, double *ad_y_out, double *ad_r_out) //プログラム使用時、now_x,now_yはグローバル変数として定義する必要あり
la00noix 0:591749f315ac 19 {
la00noix 0:591749f315ac 20 double Vector_P[2] = {(plot_x2 - plot_x1), (plot_y2 - plot_y1)}; //ベクトルAB
la00noix 0:591749f315ac 21 double A_Vector_P = hypot(Vector_P[0], Vector_P[1]); //ベクトルABの大きさ（hypot(a,b)で√（a^2+b^2）を計算できる <math.h>））
la00noix 0:591749f315ac 22 double UnitVector_P[2] = {Vector_P[0]/A_Vector_P, Vector_P[1]/A_Vector_P}; //ベクトルABの単位ベクトル
la00noix 0:591749f315ac 23 double UnitVector_Q[2] = {UnitVector_P[1], -UnitVector_P[0]}; //ベクトルCHの単位ベクトル
la00noix 0:591749f315ac 24 double Vector_R[2] = {(now_x - plot_x1), (now_y - plot_y1)}; //ベクトルAC
la00noix 0:591749f315ac 25 double diff = UnitVector_P[0]*Vector_R[1] - UnitVector_P[1]*Vector_R[0]; //機体位置と直線ABの距離（外積を用いて計算）
la00noix 0:591749f315ac 26
la00noix 0:591749f315ac 27 double VectorOut_P[2] = {p_out*UnitVector_P[0], p_out*UnitVector_P[1]}; //ベクトルABに平行方向の出力をx軸方向、y軸方向の出力に分解
la00noix 0:591749f315ac 28
la00noix 0:591749f315ac 29 ///////////////////＜XYRmotorout関数内＞以下、ベクトルABに垂直な方向の誤差を埋めるPD制御（ベクトルABに垂直方向の出力を求め、x軸方向、y軸方向の出力に分解）//////////////////////
la00noix 0:591749f315ac 30
la00noix 0:591749f315ac 31 timer.start();
la00noix 0:591749f315ac 32 now_timeQ=timer.read();
la00noix 0:591749f315ac 33 out_dutyQ=Kvq_p*diff+Kvq_d*(diff-diff_old)/(now_timeQ-old_timeQ); //ベクトルABに垂直方向の出力を決定
la00noix 0:591749f315ac 34 diff_old=diff;
la00noix 0:591749f315ac 35
yuki0701 2:32362343f091 36 if(out_dutyQ>500)out_dutyQ=500;
yuki0701 2:32362343f091 37 if(out_dutyQ<-500)out_dutyQ=-500;
la00noix 0:591749f315ac 38
la00noix 0:591749f315ac 39 old_timeQ=now_timeQ;
la00noix 0:591749f315ac 40
la00noix 0:591749f315ac 41 double VectorOut_Q[2] = {out_dutyQ*UnitVector_Q[0], out_dutyQ*UnitVector_Q[1]}; //ベクトルABに垂直方向の出力をx軸方向、y軸方向の出力に分解
la00noix 0:591749f315ac 42
la00noix 0:591749f315ac 43 ///////////////////////////////<XYRmotorout関数内＞以下、機体角度と目標角度の誤差を埋めるPD制御（旋回のための出力値を決定）//////////////////////////////////
la00noix 0:591749f315ac 44
la00noix 0:591749f315ac 45 now_timeR=timer.read();
la00noix 0:591749f315ac 46 diffangle=target_angle-now_angle;
yuki0701 1:d438093cb464 47 out_dutyR=-(Kvr_p*diffangle+Kvr_d*(diffangle-diffangle_old)/(now_timeR-old_timeR));
la00noix 0:591749f315ac 48 diffangle_old=diffangle;
la00noix 0:591749f315ac 49
yuki0701 1:d438093cb464 50 if(out_dutyR>r_out_max)out_dutyR=r_out_max;
yuki0701 1:d438093cb464 51 if(out_dutyR<-r_out_max)out_dutyR=-r_out_max;
la00noix 0:591749f315ac 52
la00noix 0:591749f315ac 53 old_timeR=now_timeR;
la00noix 0:591749f315ac 54
la00noix 0:591749f315ac 55 //////////////////////////<XYRmotorout関数内＞以下、x軸方向、y軸方向、旋回の出力をそれぞれad_x_out,ad_y_out,ad_r_outの指すアドレスに書き込む/////////////////////////////
la00noix 0:591749f315ac 56 ////////////////////////////////////////////その際、x軸方向、y軸方向の出力はフィールドの座標系から機体の座標系に変換する。///////////////////////////////////////////////
yuki0701 2:32362343f091 57 switch(type) {
la00noix 0:591749f315ac 58
yuki0701 2:32362343f091 59 case 1://減速しない運動
yuki0701 2:32362343f091 60 *ad_x_out = (VectorOut_P[0]+VectorOut_Q[0])*cos(-now_angle*3.141592/180)-(VectorOut_P[1]+VectorOut_Q[1])*sin(-now_angle*3.141592/180);
yuki0701 2:32362343f091 61 *ad_y_out = (VectorOut_P[0]+VectorOut_Q[0])*sin(-now_angle*3.141592/180)+(VectorOut_P[1]+VectorOut_Q[1])*cos(-now_angle*3.141592/180);
yuki0701 2:32362343f091 62 *ad_r_out = out_dutyR;
yuki0701 2:32362343f091 63 break;
yuki0701 2:32362343f091 64
yuki0701 2:32362343f091 65 case 2://減速する運動
yuki0701 2:32362343f091 66 diff_tgt = hypot(now_x - plot_x2, now_y - plot_y2);
yuki0701 2:32362343f091 67
yuki0701 2:32362343f091 68 if(diff_tgt > 500) {
yuki0701 2:32362343f091 69 diff_tgt = 500;
yuki0701 2:32362343f091 70 }
yuki0701 2:32362343f091 71
yuki0701 2:32362343f091 72 p_param=diff_tgt/500;
yuki0701 2:32362343f091 73
yuki0701 2:32362343f091 74 *ad_x_out = p_param*((VectorOut_P[0]+VectorOut_Q[0])*cos(-now_angle*3.141592/180)-(VectorOut_P[1]+VectorOut_Q[1])*sin(-now_angle*3.141592/180));
yuki0701 2:32362343f091 75 *ad_y_out = p_param*((VectorOut_P[0]+VectorOut_Q[0])*sin(-now_angle*3.141592/180)+(VectorOut_P[1]+VectorOut_Q[1])*cos(-now_angle*3.141592/180));
yuki0701 2:32362343f091 76 *ad_r_out = out_dutyR;
yuki0701 2:32362343f091 77 break;
yuki0701 2:32362343f091 78 }
la00noix 0:591749f315ac 79 }
la00noix 0:591749f315ac 80
la00noix 0:591749f315ac 81 ////////////////////////////////////////////////////////////<XYRmotorout関数は以上>////////////////////////////////////////////////////////////////
la00noix 0:591749f315ac 82
la00noix 0:591749f315ac 83
la00noix 0:591749f315ac 84 void set_p_out(double p) //ベクトルABに平行方向の出力値設定関数
la00noix 0:591749f315ac 85 {
la00noix 0:591749f315ac 86 p_out = p;
la00noix 0:591749f315ac 87 }
la00noix 0:591749f315ac 88
la00noix 0:591749f315ac 89 void q_setPDparam(double q_p,double q_d) //ベクトルABに垂直な方向の誤差を埋めるPD制御のパラメータ設定関数
la00noix 0:591749f315ac 90 {
la00noix 0:591749f315ac 91 Kvq_p=q_p;
la00noix 0:591749f315ac 92 Kvq_d=q_d;
la00noix 0:591749f315ac 93 }
la00noix 0:591749f315ac 94
la00noix 0:591749f315ac 95 void r_setPDparam(double r_p,double r_d) //機体角度と目標角度の誤差を埋めるPD制御のパラメータ設定関数
la00noix 0:591749f315ac 96 {
la00noix 0:591749f315ac 97 Kvr_p=r_p;
la00noix 0:591749f315ac 98 Kvr_d=r_d;
la00noix 0:591749f315ac 99 }
la00noix 0:591749f315ac 100
la00noix 0:591749f315ac 101 void set_r_out(double r) //旋回時の最大出力値設定関数
la00noix 0:591749f315ac 102 {
yuki0701 1:d438093cb464 103 r_out_max = r;
la00noix 0:591749f315ac 104 }
la00noix 0:591749f315ac 105
la00noix 0:591749f315ac 106 void set_target_angle(double t) //機体の目標角度設定関数
la00noix 0:591749f315ac 107 {
la00noix 0:591749f315ac 108 target_angle = t;
la00noix 0:591749f315ac 109 }