Diff: main.cpp

Revision:

8:ddedee42c253

Parent:

7:93ab5505ac1b

Child:

9:8c550d496736

--- a/main.cpp	Thu Mar 19 04:53:20 2020 +0000
+++ b/main.cpp	Thu Mar 19 06:53:48 2020 +0000
@@ -30,12 +30,11 @@
 /* 自己位置推定処理 */
 void LocEstimate(void);
 
-/* 台形制御値代入 */
-void omuni_control(int tar_x, int tar_y, int tar_rev);
-
 /* オムニ関係 */
 void SubOmuni(int X,int Y,int R);
 
+/* 台形制御値代入 */
+int  omuni_control(int tar_x, int tar_y, int tar_rev);
 
 /* 変数定義 ------------------------------------------------------------------*/
 
@@ -51,9 +50,6 @@
 /* 足回り値保存変数 */
 int MovMotor[4]= {0};
 
-/* 移動終了フラグ */
-bool fin_flag=0;
-
 /* 自動yaw補整目標角度 */
 double TarTheta=0;
 
@@ -191,12 +187,8 @@
 
                 /* 〇〇の処理 */
                 case 1:
-                    omuni_control(1000, 1000, 360);
                     /* 〇〇の時次のステップに */
-                    if(fin_flag) {
-                        fin_flag=0;
-                        auto_mode++;
-                    }
+                    if(omuni_control(1000, 1000, 360) == 1) auto_mode++;
                     break;
 
                 /* 終了処理 */
@@ -331,10 +323,10 @@
  * @概要   P制御
  * @引数   tar_x  :目標のx座標
  * @引数   tar_y  :目標のy座標
- * @引数   tar_rev: 目標角
+ * @引数   tar_theta: 目標角
  * @返り値 なし
 *******************************************************************************/
-void omuni_control(int tar_x, int tar_y, int tar_theta)
+int omuni_control(int tar_x, int tar_y, int tar_theta)
 {
     static int log_distance = 0;
     static double log_theta = 0;
@@ -387,7 +379,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 - log_theta);
+    theta_tar = (((double)log_distance - remain) / log_distance) * (tar_theta - log_theta) + log_theta;
     /* 合わせるthetaと現在thetaの差分からtheta補整をかける */
     t_val = (theta_tar - NowLoc.theta) * 1.5;
 
@@ -410,6 +402,8 @@
         P_fin.stop();
         /* フラグを建て直す */
         dis_flag=0;
-        fin_flag=1;
+        
+        return 1;
     }
+    return 0;
 }
\ No newline at end of file