3/1
Dependencies: mbed move4wheel2 EC CruizCore_R1370P
PathFollowing.cpp
00001 #include "PathFollowing.h" 00002 #include "mbed.h" 00003 #include "math.h" 00004 00005 double p_out,r_out_max; 00006 double Kvq_p,Kvq_d,Kvr_p,Kvr_d; 00007 double diff_old,diffangle,diffangle_old; 00008 double out_dutyQ,out_dutyR; 00009 double now_angle,target_angle; 00010 double now_timeQ,old_timeQ,now_timeR,old_timeR; 00011 double now_x, now_y; 00012 double diff_st,diff_tgt,diff_st_tgt,p_param; 00013 double usw_data1,usw_data2,usw_data3,usw_data4; 00014 00015 Timer timer; 00016 00017 //初期座標:A, 目標座標:B、機体位置:C、点Cから直線ABに下ろした垂線の足:H 00018 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はグローバル変数として定義する必要あり 00019 //plot_x1,plot_y1:出発地点の座標 00020 //plot_x2,plot_y2:目標地点の座標 00021 //speed1:初期速度 00022 //speed2:目標速度 00023 { 00024 double Vector_P[2] = {(plot_x2 - plot_x1), (plot_y2 - plot_y1)}; //ベクトルAB 00025 double A_Vector_P = hypot(Vector_P[0], Vector_P[1]); //ベクトルABの大きさ(hypot(a,b)で√(a^2+b^2)を計算できる <math.h>)) 00026 double UnitVector_P[2] = {Vector_P[0]/A_Vector_P, Vector_P[1]/A_Vector_P}; //ベクトルABの単位ベクトル 00027 double UnitVector_Q[2] = {UnitVector_P[1], -UnitVector_P[0]}; //ベクトルCHの単位ベクトル 00028 double Vector_R[2] = {(now_x - plot_x1), (now_y - plot_y1)}; //ベクトルAC 00029 double diff = UnitVector_P[0]*Vector_R[1] - UnitVector_P[1]*Vector_R[0]; //機体位置と直線ABの距離(外積を用いて計算) 00030 00031 00032 //double VectorOut_P[2]= {0}; //ベクトルABに平行方向の出力をx軸方向、y軸方向の出力に分解*/ 00033 00034 ///////////////////<XYRmotorout関数内>以下、ベクトルABに垂直な方向の誤差を埋めるPD制御(ベクトルABに垂直方向の出力を求め、x軸方向、y軸方向の出力に分解)////////////////////// 00035 00036 timer.start(); 00037 now_timeQ=timer.read(); 00038 out_dutyQ=Kvq_p*diff+Kvq_d*(diff-diff_old)/(now_timeQ-old_timeQ); //ベクトルABに垂直方向の出力を決定 00039 diff_old=diff; 00040 00041 if(out_dutyQ>500)out_dutyQ=500; 00042 if(out_dutyQ<-500)out_dutyQ=-500; 00043 00044 old_timeQ=now_timeQ; 00045 00046 double VectorOut_Q[2] = {out_dutyQ*UnitVector_Q[0], out_dutyQ*UnitVector_Q[1]}; //ベクトルABに垂直方向の出力をx軸方向、y軸方向の出力に分解 00047 00048 ///////////////////////////////<XYRmotorout関数内>以下、機体角度と目標角度の誤差を埋めるPD制御(旋回のための出力値を決定)////////////////////////////////// 00049 00050 now_timeR=timer.read(); 00051 diffangle=target_angle-now_angle; 00052 out_dutyR=-(Kvr_p*diffangle+Kvr_d*(diffangle-diffangle_old)/(now_timeR-old_timeR)); 00053 diffangle_old=diffangle; 00054 00055 if(out_dutyR>r_out_max)out_dutyR=r_out_max; 00056 if(out_dutyR<-r_out_max)out_dutyR=-r_out_max; 00057 00058 old_timeR=now_timeR; 00059 00060 //////////////////////////<XYRmotorout関数内>以下、x軸方向、y軸方向、旋回の出力をそれぞれad_x_out,ad_y_out,ad_r_outの指すアドレスに書き込む///////////////////////////// 00061 ////////////////////////////////////////////その際、x軸方向、y軸方向の出力はフィールドの座標系から機体の座標系に変換する。/////////////////////////////////////////////// 00062 00063 diff_st = hypot(now_x-plot_x1,now_y-plot_y1); //出発座標と機体の位置の距離 00064 diff_tgt = hypot(now_x - plot_x2, now_y - plot_y2); //機体の位置と目標座標の距離 00065 diff_st_tgt = hypot(plot_x1-plot_x2,plot_y1-plot_y2); //出発座標と目標座標の距離 00066 00067 if(speed1 == speed2) { //等速移動 00068 00069 double VectorOut_P[2] = {speed1*UnitVector_P[0], speed1*UnitVector_P[1]}; 00070 00071 *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); 00072 *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); 00073 *ad_r_out = out_dutyR; 00074 00075 } else if(speed2 == 0) { //減速移動(目標速度が0)→ベクトルABに垂直な方向の出力にもP制御をかける。 00076 00077 double VectorOut_P[2] = {speed1*UnitVector_P[0], speed1*UnitVector_P[1]}; 00078 00079 if(diff_tgt > diff_st_tgt) { 00080 diff_tgt = diff_st_tgt; 00081 } 00082 00083 p_param=(diff_tgt/diff_st_tgt); 00084 00085 *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)); 00086 *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)); 00087 *ad_r_out = out_dutyR; 00088 00089 } else if(speed1 > speed2) { //減速移動(目標速度が0でない) 00090 00091 if(diff_tgt > diff_st_tgt) { 00092 diff_tgt = diff_st_tgt; 00093 } 00094 00095 p_param=(diff_tgt/diff_st_tgt); 00096 00097 double speed3 = speed2 + (speed1-speed2)*p_param; 00098 00099 double VectorOut_P[2] = {speed3*UnitVector_P[0], speed3*UnitVector_P[1]}; 00100 00101 *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); 00102 *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); 00103 *ad_r_out = out_dutyR; 00104 00105 } else if(speed1 < speed2) { //加速移動(speed1) 00106 00107 if(diff_st > diff_st_tgt) { 00108 diff_st = diff_st_tgt; 00109 } 00110 00111 p_param=(diff_st/diff_st_tgt); 00112 00113 double speed4 = speed1 + (speed2-speed1)*p_param; 00114 00115 double VectorOut_P[2] = {speed4*UnitVector_P[0], speed4*UnitVector_P[1]}; 00116 00117 *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); 00118 *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); 00119 *ad_r_out = out_dutyR; 00120 } 00121 } 00122 00123 ////////////////////////////////////////////////////////////<XYRmotorout関数は以上>//////////////////////////////////////////////////////////////// 00124 00125 00126 /*void set_p_out(double p) //ベクトルABに平行方向の出力値設定関数 00127 { 00128 p_out = p; 00129 }*/ 00130 00131 void q_setPDparam(double q_p,double q_d) //ベクトルABに垂直な方向の誤差を埋めるPD制御のパラメータ設定関数 00132 { 00133 Kvq_p=q_p; 00134 Kvq_d=q_d; 00135 } 00136 00137 void r_setPDparam(double r_p,double r_d) //機体角度と目標角度の誤差を埋めるPD制御のパラメータ設定関数 00138 { 00139 Kvr_p=r_p; 00140 Kvr_d=r_d; 00141 } 00142 00143 void set_r_out(double r) //旋回時の最大出力値設定関数 00144 { 00145 r_out_max = r; 00146 } 00147 00148 void set_target_angle(double t) //機体の目標角度設定関数 00149 { 00150 target_angle = t; 00151 }
Generated on Sat Jul 16 2022 00:32:08 by 1.7.2