Diff: PathFollowing.cpp

Revision:

2:32362343f091

Parent:

1:d438093cb464

Child:

3:5da150ef209c

--- 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関数は以上>////////////////////////////////////////////////////////////////