Diff: main.cpp

Revision:

10:9ee22b22e583

Parent:

9:94112b5df540

Child:

11:42310638e241

--- a/main.cpp	Mon Mar 16 05:39:32 2020 +0000
+++ b/main.cpp	Tue Mar 17 05:04:17 2020 +0000
@@ -73,6 +73,9 @@
 /* ジャイロ用タイマー */
 Timer yawCnt;
 
+/* x座標移動タイマー */
+Timer CooCnt;
+
 /* 足回り動作クラス定義 */
 Move omuni(MovMotor,NowLoc.theta);
 
@@ -103,13 +106,13 @@
     MD.Init(1,MD_SMB);
     MD.Init(2,MD_SMB);
     MD.Init(3,MD_SMB);
-    
+
     In.Init(0,0);
     In.Init(0,1);
 
     telemetry.printf("\n\rMainLoopStart");
     /* メインループ --------------------------------------------------------------*/
-    while(1) {
+    while(1) {        
         /* オンボードLED点滅 */
         led=!led;
 
@@ -141,7 +144,7 @@
         if(operate==0) {
             /* 足回り停止 */
             omuni.XmarkOmni_Move(0,0,0);
-            
+
             for(int i=0; i<4; i++)
                 MD.Set(i,MovMotor[i]);
         }
@@ -160,10 +163,10 @@
             r_val+=(TarTheta-NowLoc.theta)*CONST_CORRECTION_YAW;
 
             omuni.XmarkOmni_Move(x_val,y_val,r_val);
-            
+
             /* 壁あて実行 */
             if(DS3.R1) ToWall();
-            
+
             for(int i=0; i<4; i++)
                 MD.Set(i,MovMotor[i]);
         }
@@ -296,4 +299,34 @@
     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;
+}
+
+
+
+/*******************************************************************************
+* @概要    x座標移動関数
+* @引数    tarpt:目標値
+* @引数    Err  :許容誤差
+* @返り値  val_p:残り距離
+*******************************************************************************/
+int XCooMove(double Tar,double Err)
+{
+    double val_p=0,TimVal=0,val=0;
+
+    /* タイマースタート */
+    CooCnt.start();
+    /* タイマーリセット */
+    CooCnt.reset();
+
+    val_p=Tar-NowLoc.X ;
+    TimVal=CooCnt.read_ms()*CONST_CORRECTION_TIME;
+
+    val=MIN(val_p,TimVal);
+    
+    if(val>100) val=100;
+    
+    omuni.XmarkOmni_Move(val,0,0);
+    
+    if(val_p<=Err) return 0;
+    else return val_p;
 }
\ No newline at end of file