Diff: main.cpp

Revision:

1:b9f698be841c

Parent:

0:9e851dc42cde

--- a/main.cpp	Thu Mar 12 02:29:33 2020 +0000
+++ b/main.cpp	Sat Mar 14 05:29:14 2020 +0000
@@ -46,11 +46,17 @@
 /* 自動yaw補整目標角度 */
 double TarTheta=0;
 
+/* 補正値用定数 */
+int cor=4;
+
 /* クラス定義 ----------------------------------------------------------------*/
 
 /* 割り込み用クラス */
 Ticker flipper;
 
+/* gyro用タイマ */
+Timer yawCnt;
+
 /* UART (Tx,Rx) */
 Serial telemetry(USBTX, USBRX, 115200);
 
@@ -115,6 +121,11 @@
 
     /* I2CMD実行 */
     MD.Exe();
+    
+    /* タイマ-スタート */
+    yawCnt.start();
+    /* タイマーリセット */
+    yawCnt.reset();
 
 
     /* 操縦権変更 ×停止 △手動 〇自動 */
@@ -126,7 +137,9 @@
     if(operate==0){
       /* 足回り停止 */
       omuni.XmarkOmni_Move(0,0,0);
-
+      for (int i = 0; i < 4; i++) {
+                MD.Set(i,MovMotor[i]);
+            }
 
     }
     /* 操縦権：手動 */
@@ -135,12 +148,18 @@
       int x_val = (double)(DS3.LX-64)*100/64;
       int y_val = (double)(64-DS3.LY)*100/64;
       int r_val = (double)(DS3.RX-64)*100/64;
-      /**
-       * このr_valとNowLoc.thetaとTarThetaをｺﾈｺﾈしてジャイロ補整するのじゃ
-       * 動けばいいのではなく，見やすく書くのじゃよ
-       **/
+
+      /* 目標角更新 */
+      if(DS3.RX!=64) yawCnt.reset(); 
+      if(yawCnt.read_ms()<1000) TarTheta=NowLoc.theta;
+      
+      /* gyro値による補正 */
+      r_val += (TarTheta-NowLoc.theta)*cor;
+       
       omuni.XmarkOmni_Move(x_val,y_val,r_val);
-
+      for (int i = 0; i < 4; i++) {
+                MD.Set(i,MovMotor[i]);
+            }
 
     }
     /* 操縦権：自動 */