Diff: main.cpp

Revision:

6:201e3de9777d

Parent:

5:1cf9b063e4fb

Child:

7:93ab5505ac1b

--- a/main.cpp	Wed Mar 18 01:32:16 2020 +0000
+++ b/main.cpp	Wed Mar 18 05:11:36 2020 +0000
@@ -48,8 +48,8 @@
 /* 足回り値保存変数 */
 int MovMotor[4]= {0};
 
-/* 総移動距離取得フラグ */
-bool dis_flag=0;
+/* 移動終了フラグ */
+bool fin_flag=0;
 
 /* 自動yaw補整目標角度 */
 double TarTheta=0;
@@ -184,8 +184,12 @@
 
                 /* 〇〇の処理 */
                 case 1:
+                    Pro_control(1000, 1000, 360);
                     /* 〇〇の時次のステップに */
-                    if(1) auto_mode++;
+                    if(fin_flag) {
+                        fin_flag=0;
+                        auto_mode++;
+                    }
                     break;
 
                 /* 終了処理 */
@@ -298,9 +302,13 @@
 *******************************************************************************/
 void Pro_control(int tar_x, int tar_y, int tar_theta)
 {
-    int log_distance = 0;
+    static int log_distance = 0;
+    static double log_theta = 0;
+    static bool dis_flag = 0;
 
     if(dis_flag==0) {
+        /* 現在角度を記録しておく */
+        log_theta = NowLoc.theta;
         /* 現在座標と目標座標との距離を記録しておく */
         log_distance = (int)sqrt(pow((double)tar_x - NowLoc.X, 2.0) + pow((double)tar_y - NowLoc.Y, 2.0));
         /* フラグ回収 */
@@ -334,7 +342,7 @@
     /* 残り距離計算 */
     int remain = (int)sqrt(pow((double)tar_x - NowLoc.X, 2.0) + pow((double)tar_y - NowLoc.Y, 2.0));
     /* 移動量に応じたthetaを求める */
-    theta_tar = (((double)log_distance - remain) / log_distance) * (tar_theta - NowLoc.theta);
+    theta_tar = (((double)log_distance - remain) / log_distance) * (tar_theta - log_theta);
     /* 合わせるthetaと現在thetaの差分からtheta補整をかける */
     t_val = (theta_tar - NowLoc.theta) * 1.5;
 
@@ -359,5 +367,6 @@
         P_fin.reset();
         /* フラグを建て直す */
         dis_flag=0;
+        fin_flag=1;
     }
 }
\ No newline at end of file