Diff: main.cpp

Revision:

16:ea3008995c41

Parent:

15:909c23ab7f0c

Child:

17:5b4080915375

--- a/main.cpp	Wed Mar 18 05:21:48 2020 +0000
+++ b/main.cpp	Wed Mar 18 09:29:44 2020 +0000
@@ -34,6 +34,12 @@
 /* x座標移動 */
 int XCooMove(int Tar,int Err);
 
+/* ライン補正 */
+void LineCor(void);
+
+/*  */
+void SubOmuni(int X,int Y,int R);
+
 /* 変数定義 ------------------------------------------------------------------*/
 
 /* 操作権 0…なし 1…手動 2…自動 */
@@ -137,6 +143,11 @@
         /* オムニ分のMD代入 */
         OmuniExe;
 
+        for(int i=0; i<4; i++) {
+            MD.Set(i,MovMotor[i]);
+            MovMotor[i]=0;
+        }
+
         /* I2CMD実行 */
         MD.Exe();
 
@@ -153,7 +164,7 @@
         /* 操縦権：なし 停止動作 */
         if(operate==0) {
             /* 足回り停止 */
-            omuni.XmarkOmni_Move(0,0,0);
+            SubOmuni(0,0,0);
 
         }
         /* 操縦権：手動 */
@@ -170,7 +181,7 @@
             /* r_val補正 */
             r_val+=(TarTheta-NowLoc.theta)*CONST_CORRECTION_YAW;
 
-            omuni.XmarkOmni_Move(x_val,y_val,r_val);
+            SubOmuni(x_val,y_val,r_val);
 
             /* 壁あて実行 */
             if(DS3.R1) ToWall();
@@ -349,7 +360,7 @@
     else if(val<-100) val=-100;
 
     /* X方向のみ操作量を与えてオムニ代入 */
-    omuni.XmarkOmni_Move(val,0,0);
+    SubOmuni(val,0,0);
 
     /* 残り距離求める */
     int RemDis=Tar-NowLoc.X;
@@ -367,6 +378,30 @@
 
 
 
+
+/*******************************************************************************
+* @概要   オムニの値計算する(加算式)
+* @引数   X,Y,Rotationそれぞれの操作量
+* @返り値 なし
+*******************************************************************************/
+void SubOmuni(int X,int Y,int R)
+{
+    /* 入力を100%に制限 */
+    X=Rest(X,100);
+    Y=Rest(Y,100);
+    R=Rest(R,100);
+
+    /* オムニ計算結果をtmpに */
+    int tmp[4]= {0};
+    omuni.XmarkOmni_Move(X,Y,R);
+
+    /* 計算結果を加算する */
+    for(int i=0; i<4; i++) MovMotor[i]+=tmp[i];
+}
+
+
+
+
 /*******************************************************************************
 * @概要    ライン補正関数
 * @引数    なし
@@ -375,7 +410,7 @@
 void LineCor(void)
 {
     /* xの操作量 */
-    int x_val=0;
+    int val_x=0;
     /* 左側,右側の前回の値 */
     int LastLf,LastRi;
     /* ライン補正初期化フラグ */
@@ -390,18 +425,18 @@
 
     /* どちらもラインに乗っていない状態 */
     if(In.Get(2)==0&&In.Get(3)==0) {
-        if(LastLf==1) x_val-=10;
-        if(LastRi==1) x_val+=10;
+        if(LastLf==1) val_x-=10;
+        if(LastRi==1) val_x+=10;
     }
-    
+
     /* どちらかがラインに乗っている状態 */
     if(In.Get(2)==0&&In.Get(3)==1) {
         /* 左側のみ */
         if(In.Get(2)==1&&In.Get(3)==0)
-            x_val-=10;
+            val_x-=10;
         /* 右側のみ */
         else if(In.Get(2)==0&&In.Get(3)==1)
-            x_val+=10;
+            val_x+=10;
         /* どちらも乗っている */
         else
             /* フラグ回収 */
@@ -413,5 +448,5 @@
         LastRi=In.Get(3);
     }
 
-    omuni.XmarkOmni_Move(x_val,0,0);
+    SubOmuni(val_x,0,0);
 }
\ No newline at end of file