Diff: main.cpp

Revision:

13:c8af1467ba8b

Parent:

12:10ce310bbdf4

Child:

14:18ec50f38993

--- a/main.cpp	Tue Mar 17 08:03:36 2020 +0000
+++ b/main.cpp	Tue Mar 17 13:32:34 2020 +0000
@@ -31,6 +31,9 @@
 /* 壁あて */
 void ToWall(void);
 
+/* x座標移動 */
+int XCooMove(int Tar,int Err);
+
 /* 変数定義 ------------------------------------------------------------------*/
 
 /* 操作権 0…なし 1…手動 2…自動 */
@@ -48,6 +51,9 @@
 /* 自動yaw補整目標角度 */
 double TarTheta=0;
 
+/* x座標移動タイマーリセットフラグ */
+bool flag=0;
+
 /* クラス定義 ----------------------------------------------------------------*/
 
 /* 割り込み用クラス */
@@ -135,10 +141,6 @@
         /* ジャイロ用タイマーリセット */
         yawCnt.reset();
 
-
-        /* X座標移動用タイマーリセット */
-        CooCnt.reset();
-
         /* 操縦権変更 ×停止 △手動 〇自動 */
         if(DS3.CROSS)     operate=0;
         if(DS3.TRIANGLE)  operate=1;
@@ -185,8 +187,10 @@
 
                 /* 〇〇の処理 */
                 case 1:
-                    /* 〇〇の時次のステップに */
-                    if(1) auto_mode++;
+                    XCooMove(1000,50);
+                    /* 動作終了時次のステップに */
+                    if(XCooMove(1000,50)==0)
+                        auto_mode++;
                     break;
 
                 /* 終了処理 */
@@ -317,12 +321,16 @@
 {
     int val_p=0,val_t=0,val=0;
 
-    /* タイマースタート */
-    CooCnt.start();
+    if(flag==0) {
+        /* タイマースタート */
+        CooCnt.start();
+        /* X座標移動用タイマーリセット */
+        CooCnt.reset();
+        flag=1;
+    }
 
-
-    val_p=Tar-NowLoc.X*CONST_CORRECTION_X;
-    val_t=CooCnt.read_ms()*CONST_CORRECTION_X;
+    val_p=(Tar-NowLoc.X)*CONST_MOVE_X;
+    val_t=CooCnt.read_ms()*CONST_MOVE_X;
 
     val=MIN(val_p,val_t);
 
@@ -332,7 +340,7 @@
 
     omuni.XmarkOmni_Move(val,0,0);
 
-    int RemDis=(int)sqrt(pow((double)Tar-NowLoc.X,2.0));
+    int RemDis=Tar-NowLoc.X;
 
     if(RemDis<=Err) return 0;
     else return RemDis;