harurobo_mbed_undercarriage_sub
Diff: PathFollowing.cpp
- Revision:
- 4:69775231687c
- Parent:
- 3:5da150ef209c
- Child:
- 6:efe1bc381434
--- a/PathFollowing.cpp Tue Dec 04 13:24:20 2018 +0000 +++ b/PathFollowing.cpp Wed Dec 05 07:01:57 2018 +0000 @@ -9,13 +9,17 @@ double now_angle,target_angle; double now_timeQ,old_timeQ,now_timeR,old_timeR; double now_x, now_y; -double diff_tgt, p_param; +double diff_st,diff_tgt,diff_st_tgt,p_param; Timer timer; //初期座標:A, 目標座標:B、機体位置:C、点Cから直線ABに下ろした垂線の足:H -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はグローバル変数として定義する必要あり +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はグローバル変数として定義する必要あり +//plot_x1,plot_y1:出発地点の座標 +//plot_x2,plot_y2:目標地点の座標 +//speed1:初期速度 +//speed2:目標速度 { double Vector_P[2] = {(plot_x2 - plot_x1), (plot_y2 - plot_y1)}; //ベクトルAB double A_Vector_P = hypot(Vector_P[0], Vector_P[1]); //ベクトルABの大きさ(hypot(a,b)で√(a^2+b^2)を計算できる <math.h>)) @@ -24,7 +28,8 @@ double Vector_R[2] = {(now_x - plot_x1), (now_y - plot_y1)}; //ベクトルAC double diff = UnitVector_P[0]*Vector_R[1] - UnitVector_P[1]*Vector_R[0]; //機体位置と直線ABの距離(外積を用いて計算) - double VectorOut_P[2] = {p_out*UnitVector_P[0], p_out*UnitVector_P[1]}; //ベクトルABに平行方向の出力をx軸方向、y軸方向の出力に分解 + + //double VectorOut_P[2]= {0}; //ベクトルABに平行方向の出力をx軸方向、y軸方向の出力に分解*/ ///////////////////<XYRmotorout関数内>以下、ベクトルABに垂直な方向の誤差を埋めるPD制御(ベクトルABに垂直方向の出力を求め、x軸方向、y軸方向の出力に分解)////////////////////// @@ -54,37 +59,74 @@ //////////////////////////<XYRmotorout関数内>以下、x軸方向、y軸方向、旋回の出力をそれぞれad_x_out,ad_y_out,ad_r_outの指すアドレスに書き込む///////////////////////////// ////////////////////////////////////////////その際、x軸方向、y軸方向の出力はフィールドの座標系から機体の座標系に変換する。/////////////////////////////////////////////// - switch(type) { + + diff_st = hypot(now_x-plot_x1,now_y-plot_y1); //出発座標と機体の位置の距離 + diff_tgt = hypot(now_x - plot_x2, now_y - plot_y2); //機体の位置と目標座標の距離 + diff_st_tgt = hypot(plot_x1-plot_x2,plot_y1-plot_y2); //出発座標と目標座標の距離 + + if(speed1 == speed2) { //等速移動 + + double VectorOut_P[2] = {speed1*UnitVector_P[0], speed1*UnitVector_P[1]}; + + *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); + *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); + *ad_r_out = out_dutyR; - case 1://減速しない運動 - *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); - *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); - *ad_r_out = out_dutyR; - break; + } else if(speed2 == 0) { //減速移動(目標速度が0)→ベクトルABに垂直な方向の出力にもP制御をかける。 + + double VectorOut_P[2] = {speed1*UnitVector_P[0], speed1*UnitVector_P[1]}; + + if(diff_tgt > diff_st_tgt) { + diff_tgt = diff_st_tgt; + } - case 2://減速する運動 - diff_tgt = hypot(now_x - plot_x2, now_y - plot_y2); + p_param=(diff_tgt/diff_st_tgt); + + *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)); + *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)); + *ad_r_out = out_dutyR; + + } else if(speed1 > speed2) { //減速移動(目標速度が0でない) - if(diff_tgt > 300) { - diff_tgt = 300; - } + if(diff_tgt > diff_st_tgt) { + diff_tgt = diff_st_tgt; + } + + p_param=(diff_tgt/diff_st_tgt); + + double speed3 = speed2 + (speed1-speed2)*p_param; + + double VectorOut_P[2] = {speed3*UnitVector_P[0], speed3*UnitVector_P[1]}; + + *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); + *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); + *ad_r_out = out_dutyR; - p_param=(diff_tgt/300); + } else if(speed1 < speed2) { //加速移動(speed1) + + if(diff_st > diff_st_tgt) { + diff_st = diff_st_tgt; + } + + p_param=(diff_st/diff_st_tgt); - *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)); - *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)); - *ad_r_out = out_dutyR; - break; + double speed4 = speed1 + (speed2-speed1)*p_param; + + double VectorOut_P[2] = {speed4*UnitVector_P[0], speed4*UnitVector_P[1]}; + + *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); + *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); + *ad_r_out = out_dutyR; } } ////////////////////////////////////////////////////////////<XYRmotorout関数は以上>//////////////////////////////////////////////////////////////// -void set_p_out(double p) //ベクトルABに平行方向の出力値設定関数 +/*void set_p_out(double p) //ベクトルABに平行方向の出力値設定関数 { p_out = p; -} +}*/ void q_setPDparam(double q_p,double q_d) //ベクトルABに垂直な方向の誤差を埋めるPD制御のパラメータ設定関数 {