Diff: main.cpp

Revision:

14:18ec50f38993

Parent:

13:c8af1467ba8b

Child:

15:909c23ab7f0c

--- a/main.cpp	Tue Mar 17 13:32:34 2020 +0000
+++ b/main.cpp	Tue Mar 17 14:08:44 2020 +0000
@@ -51,8 +51,6 @@
 /* 自動yaw補整目標角度 */
 double TarTheta=0;
 
-/* x座標移動タイマーリセットフラグ */
-bool flag=0;
 
 /* クラス定義 ----------------------------------------------------------------*/
 
@@ -79,9 +77,6 @@
 /* ジャイロ用タイマー */
 Timer yawCnt;
 
-/* x座標移動タイマー */
-Timer CooCnt;
-
 /* 足回り動作クラス定義 */
 Move omuni(MovMotor,NowLoc.theta);
 
@@ -133,6 +128,10 @@
         /* 自己位置推定更新 */
         LocEstimate();
 
+
+        /* オムニ分のMD代入 */
+        OmuniExe;
+
         /* I2CMD実行 */
         MD.Exe();
 
@@ -151,8 +150,6 @@
             /* 足回り停止 */
             omuni.XmarkOmni_Move(0,0,0);
 
-            for(int i=0; i<4; i++)
-                MD.Set(i,MovMotor[i]);
         }
         /* 操縦権：手動 */
         else if(operate==1) {
@@ -172,9 +169,6 @@
 
             /* 壁あて実行 */
             if(DS3.R1) ToWall();
-
-            for(int i=0; i<4; i++)
-                MD.Set(i,MovMotor[i]);
         }
         /* 操縦権：自動 */
         else if(operate==2) {
@@ -187,10 +181,10 @@
 
                 /* 〇〇の処理 */
                 case 1:
-                    XCooMove(1000,50);
                     /* 動作終了時次のステップに */
                     if(XCooMove(1000,50)==0)
                         auto_mode++;
+
                     break;
 
                 /* 終了処理 */
@@ -228,19 +222,18 @@
         /* 差を求める*/
         disp[2]=GyroDeg[1]-GyroDeg[0];
     }
-    /* Enc2つの差分求める */
-    for(int i=0; i<2; i++) {
+    /* Enc1つの差分求める */
+    for(int i=0; i<1; i++) {
         EncDeg[i][1]=EncDeg[i][0];
         EncDeg[i][0]=EncoderDeg[i];
         disp[i]=EncDeg[i][1]-EncDeg[i][0];
     }
     /* 差分を加速度として保存 */
     NowAcc.theta = disp[2];
-    NowAcc.X =  disp[0] * cos(NowLoc.theta) + disp[1] * sin(NowLoc.theta)+200*disp[2];
-    NowAcc.Y = -disp[0] * sin(NowLoc.theta) + disp[1] * cos(NowLoc.theta)+200*disp[2];
+    NowAcc.X =  -disp[0] * cos(NowLoc.theta);
+    
     /* 差分を累積して現在位置を保存 */
     NowLoc.X += NowAcc.X;
-    NowLoc.Y += NowAcc.Y;
     NowLoc.theta += NowAcc.theta;
 }
 
@@ -301,12 +294,13 @@
 *******************************************************************************/
 void ToWall(void)
 {
+    /* 左側リミットに応じて左2輪の操作量を決める */
     if(In.Get(0)==0)
         for(int i=0; i<2; i++) MovMotor[i]-=10;
-    else for(int i=0; i<2; i++) MovMotor[i]+=0;
+
+    /* 右側リミットに応じて左2輪の操作量を決める */
     if(In.Get(1)==0)
         for(int i=2; i<4; i++) MovMotor[i]+=10;
-    else for(int i=2; i<4; i++) MovMotor[i]+=0;
 }
 
 
@@ -319,29 +313,49 @@
 *******************************************************************************/
 int XCooMove(int Tar,int Err)
 {
+    /* x座標移動初期化フラグ */
+    static bool XCooMoveFlag=0;
+
+    /* x座標移動タイマー */
+    static Timer CooCnt;
+
+    /* 速度 加速・減速・最終操作量 */
     int val_p=0,val_t=0,val=0;
 
-    if(flag==0) {
+    /* X座標移動最初だけ初期化 */
+    if(XCooMoveFlag==0) {
         /* タイマースタート */
         CooCnt.start();
         /* X座標移動用タイマーリセット */
         CooCnt.reset();
-        flag=1;
+        /* 初期化フラグ立てる */
+        XCooMoveFlag=1;
     }
 
-    val_p=(Tar-NowLoc.X)*CONST_MOVE_X;
-    val_t=CooCnt.read_ms()*CONST_MOVE_X;
+    /* 加速・減速それぞれ求める */
+    val_p=(Tar-NowLoc.X)*CONST_ACC_X;
+    val_t=CooCnt.read_ms()*CONST_DEC_X;
 
+    /* 小さい方を操作量として採択 */
     val=MIN(val_p,val_t);
 
     /* -100~100に調整 */
     if(val>100) val=100;
     else if(val<-100) val=-100;
 
+    /* X方向のみ操作量を与えてオムニ代入 */
     omuni.XmarkOmni_Move(val,0,0);
 
+    /* 残り距離求める */
     int RemDis=Tar-NowLoc.X;
 
-    if(RemDis<=Err) return 0;
-    else return RemDis;
+    /* 終了条件満たしているなら終了処理 */
+    if(RemDis<=Err) {
+        /* フラグ回収 */
+        XCooMoveFlag=0;
+        return 0;
+    }
+    /* 満たしてないなら残り距離返す */
+    else
+        return RemDis;
 }
\ No newline at end of file