harurobo_mbed_undercarriage_sub
Diff: PathFollowing.cpp
- Revision:
- 1:d438093cb464
- Parent:
- 0:591749f315ac
- Child:
- 2:32362343f091
diff -r 591749f315ac -r d438093cb464 PathFollowing.cpp --- a/PathFollowing.cpp Fri Nov 16 23:16:59 2018 +0000 +++ b/PathFollowing.cpp Sat Nov 24 14:44:14 2018 +0000 @@ -2,7 +2,7 @@ #include <mbed.h> #include <math.h> -double p_out,r_out; +double p_out,r_out_max; double Kvq_p,Kvq_d,Kvr_p,Kvr_d; double diff_old,diffangle,diffangle_old; double out_dutyQ,out_dutyR; @@ -10,6 +10,7 @@ double now_timeQ,old_timeQ,now_timeR,old_timeR; double now_x, now_y; + Timer timer; //初期座標:A, 目標座標:B、機体位置:C、点Cから直線ABに下ろした垂線の足:H @@ -42,19 +43,19 @@ now_timeR=timer.read(); diffangle=target_angle-now_angle; - out_dutyR=Kvr_p*diff+Kvr_d*(diffangle-diffangle_old)/(now_timeR-old_timeR); + out_dutyR=-(Kvr_p*diffangle+Kvr_d*(diffangle-diffangle_old)/(now_timeR-old_timeR)); diffangle_old=diffangle; - if(out_dutyR>r_out)out_dutyR=r_out; - if(out_dutyR<-r_out)out_dutyR=-r_out; + if(out_dutyR>r_out_max)out_dutyR=r_out_max; + if(out_dutyR<-r_out_max)out_dutyR=-r_out_max; old_timeR=now_timeR; //////////////////////////<XYRmotorout関数内>以下、x軸方向、y軸方向、旋回の出力をそれぞれad_x_out,ad_y_out,ad_r_outの指すアドレスに書き込む///////////////////////////// ////////////////////////////////////////////その際、x軸方向、y軸方向の出力はフィールドの座標系から機体の座標系に変換する。/////////////////////////////////////////////// - *ad_x_out = (VectorOut_P[0]+VectorOut_Q[0])*cos(now_angle)-(VectorOut_P[1]+VectorOut_Q[1])*sin(now_angle); - *ad_y_out = (VectorOut_P[0]+VectorOut_Q[0])*sin(now_angle)+(VectorOut_P[1]+VectorOut_Q[1])*cos(now_angle); + *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; } @@ -80,7 +81,7 @@ void set_r_out(double r) //旋回時の最大出力値設定関数 { - r_out = r; + r_out_max = r; } void set_target_angle(double t) //機体の目標角度設定関数