Diff: pathfollowing/PathFollowing.cpp

Revision:

6:26724c287387

Parent:

0:c61c6e4775ca

diff -r 6cebe1c458a9 -r 26724c287387 pathfollowing/PathFollowing.cpp
--- a/pathfollowing/PathFollowing.cpp	Sat Mar 02 07:18:38 2019 +0000
+++ b/pathfollowing/PathFollowing.cpp	Sat Mar 02 07:48:18 2019 +0000
@@ -28,8 +28,7 @@
     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]= {0}; //ベクトルABに平行方向の出力をx軸方向、y軸方向の出力に分解*/
+    //double VectorOut_P[2]= {0}; //ベクトルABに平行方向の出力をx軸方向、y軸方向の出力に分解
 
 ///////////////////＜XYRmotorout関数内＞以下、ベクトルABに垂直な方向の誤差を埋めるPD制御（ベクトルABに垂直方向の出力を求め、x軸方向、y軸方向の出力に分解）//////////////////////
 
@@ -91,13 +90,13 @@
         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;
@@ -107,13 +106,13 @@
         if(diff_st > diff_st_tgt) {
             diff_st = diff_st_tgt;
         }
-             
+
         p_param=(diff_st/diff_st_tgt);
 
         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;