harurobo_mbed_undercarriage_sub
Diff: PathFollowing.cpp
- Revision:
- 2:32362343f091
- Parent:
- 1:d438093cb464
- Child:
- 3:5da150ef209c
diff -r d438093cb464 -r 32362343f091 PathFollowing.cpp --- a/PathFollowing.cpp Sat Nov 24 14:44:14 2018 +0000 +++ b/PathFollowing.cpp Sat Dec 01 05:17:07 2018 +0000 @@ -9,12 +9,13 @@ double now_angle,target_angle; double now_timeQ,old_timeQ,now_timeR,old_timeR; double now_x, now_y; +double diff_tgt, p_param; Timer timer; //初期座標:A, 目標座標:B、機体位置:C、点Cから直線ABに下ろした垂線の足:H -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) //プログラム使用時、now_x,now_yはグローバル変数として定義する必要あり +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はグローバル変数として定義する必要あり { 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>)) @@ -32,8 +33,8 @@ out_dutyQ=Kvq_p*diff+Kvq_d*(diff-diff_old)/(now_timeQ-old_timeQ); //ベクトルABに垂直方向の出力を決定 diff_old=diff; - if(out_dutyQ>0.1)out_dutyQ=0.1; - if(out_dutyQ<-0.1)out_dutyQ=-0.1; + if(out_dutyQ>500)out_dutyQ=500; + if(out_dutyQ<-500)out_dutyQ=-500; old_timeQ=now_timeQ; @@ -53,10 +54,28 @@ //////////////////////////<XYRmotorout関数内>以下、x軸方向、y軸方向、旋回の出力をそれぞれad_x_out,ad_y_out,ad_r_outの指すアドレスに書き込む///////////////////////////// ////////////////////////////////////////////その際、x軸方向、y軸方向の出力はフィールドの座標系から機体の座標系に変換する。/////////////////////////////////////////////// + switch(type) { - *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; + + case 2://減速する運動 + diff_tgt = hypot(now_x - plot_x2, now_y - plot_y2); + + if(diff_tgt > 500) { + diff_tgt = 500; + } + + p_param=diff_tgt/500; + + *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; + } } ////////////////////////////////////////////////////////////<XYRmotorout関数は以上>////////////////////////////////////////////////////////////////