春ロボ1班(元F3RC4班+) / Mbed 2 deprecated harurobo_ashi_3_19

3/19

Dependencies:   mbed move4wheel2 EC CruizCore_R1370P

pathfollowing/PathFollowing.cpp@0:c61c6e4775ca, 2019-02-13 (annotated)

Committer:
la00noix
Date:
Wed Feb 13 03:02:19 2019 +0000
Revision:
0:c61c6e4775ca
Child:
6:26724c287387
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
la00noix 0:c61c6e4775ca 1 #include "PathFollowing.h"
la00noix 0:c61c6e4775ca 2 #include "mbed.h"
la00noix 0:c61c6e4775ca 3 #include "math.h"
la00noix 0:c61c6e4775ca 4
la00noix 0:c61c6e4775ca 5 double p_out,r_out_max;
la00noix 0:c61c6e4775ca 6 double Kvq_p,Kvq_d,Kvr_p,Kvr_d;
la00noix 0:c61c6e4775ca 7 double diff_old,diffangle,diffangle_old;
la00noix 0:c61c6e4775ca 8 double out_dutyQ,out_dutyR;
la00noix 0:c61c6e4775ca 9 double now_angle,target_angle;
la00noix 0:c61c6e4775ca 10 double now_timeQ,old_timeQ,now_timeR,old_timeR;
la00noix 0:c61c6e4775ca 11 double now_x, now_y;
la00noix 0:c61c6e4775ca 12 double diff_st,diff_tgt,diff_st_tgt,p_param;
la00noix 0:c61c6e4775ca 13 double usw_data1,usw_data2,usw_data3,usw_data4;
la00noix 0:c61c6e4775ca 14
la00noix 0:c61c6e4775ca 15 Timer timer;
la00noix 0:c61c6e4775ca 16
la00noix 0:c61c6e4775ca 17 //初期座標：A, 目標座標：B、機体位置：C、点Cから直線ABに下ろした垂線の足：H
la00noix 0:c61c6e4775ca 18 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, double speed1, double speed2 ) //プログラム使用時、now_x,now_yはグローバル変数として定義する必要あり
la00noix 0:c61c6e4775ca 19 //plot_x1,plot_y1:出発地点の座標
la00noix 0:c61c6e4775ca 20 //plot_x2,plot_y2:目標地点の座標
la00noix 0:c61c6e4775ca 21 //speed1:初期速度
la00noix 0:c61c6e4775ca 22 //speed2:目標速度
la00noix 0:c61c6e4775ca 23 {
la00noix 0:c61c6e4775ca 24 double Vector_P[2] = {(plot_x2 - plot_x1), (plot_y2 - plot_y1)}; //ベクトルAB
la00noix 0:c61c6e4775ca 25 double A_Vector_P = hypot(Vector_P[0], Vector_P[1]); //ベクトルABの大きさ（hypot(a,b)で√（a^2+b^2）を計算できる <math.h>））
la00noix 0:c61c6e4775ca 26 double UnitVector_P[2] = {Vector_P[0]/A_Vector_P, Vector_P[1]/A_Vector_P}; //ベクトルABの単位ベクトル
la00noix 0:c61c6e4775ca 27 double UnitVector_Q[2] = {UnitVector_P[1], -UnitVector_P[0]}; //ベクトルCHの単位ベクトル
la00noix 0:c61c6e4775ca 28 double Vector_R[2] = {(now_x - plot_x1), (now_y - plot_y1)}; //ベクトルAC
la00noix 0:c61c6e4775ca 29 double diff = UnitVector_P[0]*Vector_R[1] - UnitVector_P[1]*Vector_R[0]; //機体位置と直線ABの距離（外積を用いて計算）
la00noix 0:c61c6e4775ca 30
la00noix 0:c61c6e4775ca 31
la00noix 0:c61c6e4775ca 32 //double VectorOut_P[2]= {0}; //ベクトルABに平行方向の出力をx軸方向、y軸方向の出力に分解*/
la00noix 0:c61c6e4775ca 33
la00noix 0:c61c6e4775ca 34 ///////////////////＜XYRmotorout関数内＞以下、ベクトルABに垂直な方向の誤差を埋めるPD制御（ベクトルABに垂直方向の出力を求め、x軸方向、y軸方向の出力に分解）//////////////////////
la00noix 0:c61c6e4775ca 35
la00noix 0:c61c6e4775ca 36 timer.start();
la00noix 0:c61c6e4775ca 37 now_timeQ=timer.read();
la00noix 0:c61c6e4775ca 38 out_dutyQ=Kvq_p*diff+Kvq_d*(diff-diff_old)/(now_timeQ-old_timeQ); //ベクトルABに垂直方向の出力を決定
la00noix 0:c61c6e4775ca 39 diff_old=diff;
la00noix 0:c61c6e4775ca 40
la00noix 0:c61c6e4775ca 41 if(out_dutyQ>500)out_dutyQ=500;
la00noix 0:c61c6e4775ca 42 if(out_dutyQ<-500)out_dutyQ=-500;
la00noix 0:c61c6e4775ca 43
la00noix 0:c61c6e4775ca 44 old_timeQ=now_timeQ;
la00noix 0:c61c6e4775ca 45
la00noix 0:c61c6e4775ca 46 double VectorOut_Q[2] = {out_dutyQ*UnitVector_Q[0], out_dutyQ*UnitVector_Q[1]}; //ベクトルABに垂直方向の出力をx軸方向、y軸方向の出力に分解
la00noix 0:c61c6e4775ca 47
la00noix 0:c61c6e4775ca 48 ///////////////////////////////<XYRmotorout関数内＞以下、機体角度と目標角度の誤差を埋めるPD制御（旋回のための出力値を決定）//////////////////////////////////
la00noix 0:c61c6e4775ca 49
la00noix 0:c61c6e4775ca 50 now_timeR=timer.read();
la00noix 0:c61c6e4775ca 51 diffangle=target_angle-now_angle;
la00noix 0:c61c6e4775ca 52 out_dutyR=-(Kvr_p*diffangle+Kvr_d*(diffangle-diffangle_old)/(now_timeR-old_timeR));
la00noix 0:c61c6e4775ca 53 diffangle_old=diffangle;
la00noix 0:c61c6e4775ca 54
la00noix 0:c61c6e4775ca 55 if(out_dutyR>r_out_max)out_dutyR=r_out_max;
la00noix 0:c61c6e4775ca 56 if(out_dutyR<-r_out_max)out_dutyR=-r_out_max;
la00noix 0:c61c6e4775ca 57
la00noix 0:c61c6e4775ca 58 old_timeR=now_timeR;
la00noix 0:c61c6e4775ca 59
la00noix 0:c61c6e4775ca 60 //////////////////////////<XYRmotorout関数内＞以下、x軸方向、y軸方向、旋回の出力をそれぞれad_x_out,ad_y_out,ad_r_outの指すアドレスに書き込む/////////////////////////////
la00noix 0:c61c6e4775ca 61 ////////////////////////////////////////////その際、x軸方向、y軸方向の出力はフィールドの座標系から機体の座標系に変換する。///////////////////////////////////////////////
la00noix 0:c61c6e4775ca 62
la00noix 0:c61c6e4775ca 63 diff_st = hypot(now_x-plot_x1,now_y-plot_y1); //出発座標と機体の位置の距離
la00noix 0:c61c6e4775ca 64 diff_tgt = hypot(now_x - plot_x2, now_y - plot_y2); //機体の位置と目標座標の距離
la00noix 0:c61c6e4775ca 65 diff_st_tgt = hypot(plot_x1-plot_x2,plot_y1-plot_y2); //出発座標と目標座標の距離
la00noix 0:c61c6e4775ca 66
la00noix 0:c61c6e4775ca 67 if(speed1 == speed2) { //等速移動
la00noix 0:c61c6e4775ca 68
la00noix 0:c61c6e4775ca 69 double VectorOut_P[2] = {speed1*UnitVector_P[0], speed1*UnitVector_P[1]};
la00noix 0:c61c6e4775ca 70
la00noix 0:c61c6e4775ca 71 *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);
la00noix 0:c61c6e4775ca 72 *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);
la00noix 0:c61c6e4775ca 73 *ad_r_out = out_dutyR;
la00noix 0:c61c6e4775ca 74
la00noix 0:c61c6e4775ca 75 } else if(speed2 == 0) { //減速移動(目標速度が０)→ベクトルABに垂直な方向の出力にもP制御をかける。
la00noix 0:c61c6e4775ca 76
la00noix 0:c61c6e4775ca 77 double VectorOut_P[2] = {speed1*UnitVector_P[0], speed1*UnitVector_P[1]};
la00noix 0:c61c6e4775ca 78
la00noix 0:c61c6e4775ca 79 if(diff_tgt > diff_st_tgt) {
la00noix 0:c61c6e4775ca 80 diff_tgt = diff_st_tgt;
la00noix 0:c61c6e4775ca 81 }
la00noix 0:c61c6e4775ca 82
la00noix 0:c61c6e4775ca 83 p_param=(diff_tgt/diff_st_tgt);
la00noix 0:c61c6e4775ca 84
la00noix 0:c61c6e4775ca 85 *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));
la00noix 0:c61c6e4775ca 86 *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));
la00noix 0:c61c6e4775ca 87 *ad_r_out = out_dutyR;
la00noix 0:c61c6e4775ca 88
la00noix 0:c61c6e4775ca 89 } else if(speed1 > speed2) { //減速移動（目標速度が０でない）
la00noix 0:c61c6e4775ca 90
la00noix 0:c61c6e4775ca 91 if(diff_tgt > diff_st_tgt) {
la00noix 0:c61c6e4775ca 92 diff_tgt = diff_st_tgt;
la00noix 0:c61c6e4775ca 93 }
la00noix 0:c61c6e4775ca 94
la00noix 0:c61c6e4775ca 95 p_param=(diff_tgt/diff_st_tgt);
la00noix 0:c61c6e4775ca 96
la00noix 0:c61c6e4775ca 97 double speed3 = speed2 + (speed1-speed2)*p_param;
la00noix 0:c61c6e4775ca 98
la00noix 0:c61c6e4775ca 99 double VectorOut_P[2] = {speed3*UnitVector_P[0], speed3*UnitVector_P[1]};
la00noix 0:c61c6e4775ca 100
la00noix 0:c61c6e4775ca 101 *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);
la00noix 0:c61c6e4775ca 102 *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);
la00noix 0:c61c6e4775ca 103 *ad_r_out = out_dutyR;
la00noix 0:c61c6e4775ca 104
la00noix 0:c61c6e4775ca 105 } else if(speed1 < speed2) { //加速移動(speed1)
la00noix 0:c61c6e4775ca 106
la00noix 0:c61c6e4775ca 107 if(diff_st > diff_st_tgt) {
la00noix 0:c61c6e4775ca 108 diff_st = diff_st_tgt;
la00noix 0:c61c6e4775ca 109 }
la00noix 0:c61c6e4775ca 110
la00noix 0:c61c6e4775ca 111 p_param=(diff_st/diff_st_tgt);
la00noix 0:c61c6e4775ca 112
la00noix 0:c61c6e4775ca 113 double speed4 = speed1 + (speed2-speed1)*p_param;
la00noix 0:c61c6e4775ca 114
la00noix 0:c61c6e4775ca 115 double VectorOut_P[2] = {speed4*UnitVector_P[0], speed4*UnitVector_P[1]};
la00noix 0:c61c6e4775ca 116
la00noix 0:c61c6e4775ca 117 *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);
la00noix 0:c61c6e4775ca 118 *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);
la00noix 0:c61c6e4775ca 119 *ad_r_out = out_dutyR;
la00noix 0:c61c6e4775ca 120 }
la00noix 0:c61c6e4775ca 121 }
la00noix 0:c61c6e4775ca 122
la00noix 0:c61c6e4775ca 123 ////////////////////////////////////////////////////////////<XYRmotorout関数は以上>////////////////////////////////////////////////////////////////
la00noix 0:c61c6e4775ca 124
la00noix 0:c61c6e4775ca 125
la00noix 0:c61c6e4775ca 126 /*void set_p_out(double p) //ベクトルABに平行方向の出力値設定関数
la00noix 0:c61c6e4775ca 127 {
la00noix 0:c61c6e4775ca 128 p_out = p;
la00noix 0:c61c6e4775ca 129 }*/
la00noix 0:c61c6e4775ca 130
la00noix 0:c61c6e4775ca 131 void q_setPDparam(double q_p,double q_d) //ベクトルABに垂直な方向の誤差を埋めるPD制御のパラメータ設定関数
la00noix 0:c61c6e4775ca 132 {
la00noix 0:c61c6e4775ca 133 Kvq_p=q_p;
la00noix 0:c61c6e4775ca 134 Kvq_d=q_d;
la00noix 0:c61c6e4775ca 135 }
la00noix 0:c61c6e4775ca 136
la00noix 0:c61c6e4775ca 137 void r_setPDparam(double r_p,double r_d) //機体角度と目標角度の誤差を埋めるPD制御のパラメータ設定関数
la00noix 0:c61c6e4775ca 138 {
la00noix 0:c61c6e4775ca 139 Kvr_p=r_p;
la00noix 0:c61c6e4775ca 140 Kvr_d=r_d;
la00noix 0:c61c6e4775ca 141 }
la00noix 0:c61c6e4775ca 142
la00noix 0:c61c6e4775ca 143 void set_r_out(double r) //旋回時の最大出力値設定関数
la00noix 0:c61c6e4775ca 144 {
la00noix 0:c61c6e4775ca 145 r_out_max = r;
la00noix 0:c61c6e4775ca 146 }
la00noix 0:c61c6e4775ca 147
la00noix 0:c61c6e4775ca 148 void set_target_angle(double t) //機体の目標角度設定関数
la00noix 0:c61c6e4775ca 149 {
la00noix 0:c61c6e4775ca 150 target_angle = t;
la00noix 0:c61c6e4775ca 151 }