Diff: main.cpp

Revision:

11:42310638e241

Parent:

10:9ee22b22e583

Child:

12:10ce310bbdf4

--- a/main.cpp	Tue Mar 17 05:04:17 2020 +0000
+++ b/main.cpp	Tue Mar 17 07:46:20 2020 +0000
@@ -112,7 +112,7 @@
 
     telemetry.printf("\n\rMainLoopStart");
     /* メインループ --------------------------------------------------------------*/
-    while(1) {        
+    while(1) {
         /* オンボードLED点滅 */
         led=!led;
 
@@ -130,11 +130,15 @@
         /* I2CMD実行 */
         MD.Exe();
 
-        /* タイマースタート */
+        /* ジャイロ用タイマースタート */
         yawCnt.start();
-        /* タイマーリセット */
+        /* ジャイロ用タイマーリセット */
         yawCnt.reset();
 
+
+        /* X座標移動用タイマーリセット */
+        CooCnt.reset();
+
         /* 操縦権変更 ×停止 △手動 〇自動 */
         if(DS3.CROSS)     operate=0;
         if(DS3.TRIANGLE)  operate=1;
@@ -305,28 +309,30 @@
 
 /*******************************************************************************
 * @概要    x座標移動関数
-* @引数    tarpt:目標値
+* @引数    Tar:目標値
 * @引数    Err  :許容誤差
 * @返り値  val_p:残り距離
 *******************************************************************************/
-int XCooMove(double Tar,double Err)
+int XCooMove(int Tar,int Err)
 {
-    double val_p=0,TimVal=0,val=0;
+    int val_p=0,val_t=0,val=0;
 
     /* タイマースタート */
     CooCnt.start();
-    /* タイマーリセット */
-    CooCnt.reset();
 
-    val_p=Tar-NowLoc.X ;
-    TimVal=CooCnt.read_ms()*CONST_CORRECTION_TIME;
+    val_p=Tar-NowLoc.X*CONST_CORRECTION_X;
+    val_t=CooCnt.read_ms()*CONST_CORRECTION_X;
+
+    val=MIN(val_p,val_t);
 
-    val=MIN(val_p,TimVal);
-    
+    /* -100~100に調整 */
     if(val>100) val=100;
-    
+    else if(val<-100) val=-100;
+
     omuni.XmarkOmni_Move(val,0,0);
-    
-    if(val_p<=Err) return 0;
-    else return val_p;
+
+    int RemDis=(int)sqrt(pow((double)Tar-NowLoc.X,2.0));
+
+    if(RemDis<=Err) return 0;
+    else return RemDis;
 }
\ No newline at end of file