![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
3/2
Dependencies: mbed move4wheel2 EC CruizCore_R1370P
pathfollowing/PathFollowing.cpp@5:6cebe1c458a9, 2019-03-02 (annotated)
- Committer:
- yuki0701
- Date:
- Sat Mar 02 07:18:38 2019 +0000
- Revision:
- 5:6cebe1c458a9
- Parent:
- 0:c61c6e4775ca
a
Who changed what in which revision?
User | Revision | Line number | New 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) { //減速移動(目標速度が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) { //減速移動(目標速度が0でない) |
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 | } |