Diff: PathFollowing.cpp

Revision:

1:d438093cb464

Parent:

0:591749f315ac

Child:

2:32362343f091

--- 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)  //機体の目標角度設定関数