Diff: main.cpp

Revision:

4:05a6bda2e11f

Parent:

3:b77f51108bf1

Child:

5:1cf9b063e4fb

--- a/main.cpp	Tue Mar 17 06:05:28 2020 +0000
+++ b/main.cpp	Wed Mar 18 00:33:42 2020 +0000
@@ -48,6 +48,9 @@
 /* 足回り値保存変数 */
 int MovMotor[4]= {0};
 
+/* 総移動距離取得フラグ */
+bool dis_flag=0;
+
 /* 自動yaw補整目標角度 */
 double TarTheta=0;
 
@@ -297,65 +300,67 @@
 {
     int log_distance = 0;
 
-    /* 現在座標と目標座標との距離を記録しておく */
-    log_distance = (int)sqrt(pow((double)tar_x - NowLoc.X, 2.0) + pow((double)tar_y - NowLoc.Y, 2.0));
-
-    int x_vel = 0;
-    int y_vel = 0;
-    int t_vel = 0;
+    if(dis_flag==0) {
+        /* 現在座標と目標座標との距離を記録しておく */
+        log_distance = (int)sqrt(pow((double)tar_x - NowLoc.X, 2.0) + pow((double)tar_y - NowLoc.Y, 2.0));
+        /* フラグ回収 */
+        dis_flag=1;
+    }
 
-    double theta_tar=0.0;
+    int x_val = 0;
+    int y_val = 0;
+    int t_val = 0;
+
+    double theta_tar = 0.0;
 
-    while(1) {
-        /* 自己位置更新 */
-        LocEstimate();
+    /* 自己位置更新 */
+    LocEstimate();
 
-        x_vel = ABS((tar_x - NowLoc.X) * 0.06);
-        y_vel = ABS((tar_y - NowLoc.Y) * 0.06);
+    x_val = ABS((tar_x - NowLoc.X) * 0.06);
+    y_val = ABS((tar_y - NowLoc.Y) * 0.06);
 
-        /* 最低を決める */
-        if (x_vel < 20)
-            x_vel = 20;
-        if (y_vel < 20)
-            y_vel = 20;
-        /* 100以内に収める */
-        x_vel = Rest(x_vel, 100);
-        y_vel = Rest(y_vel, 100);
+    /* 最低を決める */
+    if (x_val < 20)
+        x_val = 20;
+    if (y_val < 20)
+        y_val = 20;
+    /* 100以内に収める */
+    x_val = Rest(x_val, 100);
+    y_val = Rest(y_val, 100);
 
-        /* 正負を決める */
-        if ((tar_x - NowLoc.X) < 0)
-            x_vel *= -1;
-        if ((tar_y - NowLoc.Y) < 0)
-            y_vel *= -1;
+    /* 正負を決める */
+    if ((tar_x - NowLoc.X) < 0)
+        x_val *= -1;
+    if ((tar_y - NowLoc.Y) < 0)
+        y_val *= -1;
 
-        /* 残り距離計算 */
-        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) + NowLoc.theta;;
-        /* 合わせるthetaと現在thetaの差分からtheta補整をかける */
-        t_vel = (theta_tar - NowLoc.theta) * 1.5;
+    /* 残り距離計算 */
+    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と現在thetaの差分からtheta補整をかける */
+    t_val = (theta_tar - NowLoc.theta) * 1.5;
 
-        t_vel = Rest(t_vel, 20);
+    t_val = Rest(t_val, 20);
 
-        omuni.XmarkOmni_Move(x_vel,y_vel,t_vel);
+    omuni.XmarkOmni_Move(x_val,y_val,t_val);
 
 
-        /* 終了処理 */
-        if ((5 > ABS(remain)) && ABS(tar_theta - NowLoc.theta) < 0.1) {
-            x_vel = 0;
-            y_vel = 0;
-        }
-        if (x_vel == 0 && y_vel == 0)
-            P_fin.start();
-        else
-            P_fin.reset();
+    /* 終了処理 */
+    if ((5 > ABS(remain)) && ABS(tar_theta - NowLoc.theta) < 0.1) {
+        x_val = 0;
+        y_val = 0;
+    }
+    if (x_val == 0 && y_val == 0)
+        P_fin.start();
+    else
+        P_fin.reset();
 
-        if (P_fin.read_ms() > 100) {
-            /* タイマー停止 */
-            P_fin.stop();
-            P_fin.reset();
-
-            break;
-        }
+    if (P_fin.read_ms() > 100) {
+        /* タイマー停止 */
+        P_fin.stop();
+        P_fin.reset();
+        /* フラグを建て直す */
+        dis_flag=0;
     }
 }
\ No newline at end of file