Revision 23:98b634305bab, committed 2020-03-24

Comitter:

yoshidayuito

Date:

Tue Mar 24 04:02:57 2020 +0000

Parent:

22:9d77148a3f09

Commit message:

Fix KyoRobo Move;Ver.yoshida

Changed in this revision

--- a/main.cpp	Mon Mar 23 05:34:40 2020 +0000
+++ b/main.cpp	Tue Mar 24 04:02:57 2020 +0000
@@ -29,13 +29,13 @@
 void LocEstimate(void);
 
 /* 壁あて */
-void ToWall(void);
+int ToWall(void);
 
 /* x座標移動 */
 int XCooMove(int Tar,int Err);
 
 /* ライン補正 */
-int LineCor(int Line_Dire);
+int LineCor(void);
 
 /* オムニ加算式 */
 void SubOmuni(int X,int Y,int R);
@@ -57,12 +57,12 @@
 /* 自動yaw補整目標角度 */
 double TarTheta=0;
 
-/* ライン補正移動方向 0…左 1…右 2…直進*/
-int Line_Dire=0;
-
 /* ライン番号 */
 int Line_Num=0;
 
+/* ラインセンサ左,右の前回の値 0…左 1…右 2…直進*/
+int Last_val=0;
+
 /* クラス定義 ----------------------------------------------------------------*/
 
 /* 割り込み用クラス */
@@ -126,6 +126,10 @@
     In.Init(0,0);
     In.Init(0,1);
 
+    /* 正面リミット */
+    In.Init(0,2);
+    In.Init(0,3);
+
     /* ラインセンサ */
     In.Init(0,2);
     In.Init(0,3);
@@ -229,21 +233,22 @@
                 /* ラインまで移動 */
                 case 2:
                     /* ライン到達後次のステップに */
-                    Line_Dire=0;
-                    if(LineCor(Line_Dire)==1)
+                    Last_val=0;
+                    if(LineCor()==1)
                         auto_mode++;
 
                     break;
                 /*  */
                 case 3:
                     /* ライン補正しながら直進 */
-                    Line_Dire=2;
-                    LineCor(Line_Dire);
+                    Last_val=2;
+                    LineCor();
                     Forward.start();
                     SubOmuni(0,20,0);
                     /* 動作終了時次のステップに */
-                    if(Forward.read_ms()==2500) {
+                    if(Forward.read_ms()<=2500) {
                         auto_mode++;
+                        Forward.stop();
                         Forward.reset();
                     }
 
@@ -251,9 +256,8 @@
 
                 case 4:
                     /* ライン補正しながら後ろへ移動 */
-                    LineCor(Line_Dire);
-                    ToWall();
-                    if(In.Get(0)==1&&In.Get(1)==1)
+                    Last_val=2;
+                    if(ToWall()&&LineCor())
                         auto_mode++;
 
                     break;
@@ -276,8 +280,9 @@
                     Forward.start();
                     SubOmuni(0,20,0);
                     /* 動作終了時次のステップに */
-                    if(Forward.read_ms()==4000) {
+                    if(Forward.read_ms()>=4000) {
                         auto_mode++;
+                        Forward.stop();
                         Forward.reset();
                     }
 
@@ -285,24 +290,23 @@
 
                 case 7:
                     /* 宝物置き場のラインまで移動 */
-                    Line_Dire=1;
-                    if(LineCor(Line_Dire)==1)
+                    Last_val=1;
+                    if(LineCor()==1)//*
                         auto_mode++;
-                        
+
                     break;
-                        
-                case 8:
-                    /* 宝物置き場前まで移動 */
-                    Forward.start();
+
+                case 8:/* ライン補正しながら直進 */
+                    Last_val=2;
+                    LineCor();
                     SubOmuni(0,20,0);
                     /* 動作終了時次のステップに */
-                    if(Forward.read_ms()==2000) {
+                    if(In.Get(0)==1&&In.Get(1)==1) {
                         auto_mode++;
-                        Forward.reset();
                     }
-                    
+
                     break;
-                    
+
                 case 9:
                     /* スタートゾーンに移動 */
                     ToWall();
@@ -418,9 +422,10 @@
 /*******************************************************************************
 * @概要   壁あて関数
 * @引数   なし
-* @返り値 なし
+* @返り値 0:未到達
+* @返り値 1:到達
 *******************************************************************************/
-void ToWall(void)
+int ToWall()
 {
     /* 左側リミットに応じて左2輪の操作量を決める */
     if(In.Get(0)==0)
@@ -430,13 +435,17 @@
     if(In.Get(1)==0)
         for(int i=2; i<4; i++) MovMotor[i]+=10;
 
+    if(In.Get(0)==1&&In.Get(1)==1)
+        return 1;
+    else return 0;
+
 }
 
 
 
 /*******************************************************************************
 * @概要    x座標移動関数
-* @引数    Tar:目標値
+* @引数    Tar  :目標値
 * @引数    Err  :許容誤差
 * @返り値  val_p:残り距離
 *******************************************************************************/
@@ -527,32 +536,22 @@
 
 /*******************************************************************************
 * @概要    ライン補正関数
-* @引数    Line_Dire:初期移動方向
+* @引数    なし
 * @返り値  なし
 *******************************************************************************/
-int LineCor(int Line_Dire)
+int LineCor(void)
 {
     /* xの操作量 */
     int val_x=0;
-    /* 左側,右側の前回の値 */
-    static int Last_val;
-    /* ライン補正初期化フラグ */
+    /* ライン到達フラグ */
     static bool LineFlag=0;
 
-    if(LineFlag==0) {
-        /* 初期値設定 0…左 1…右 2…直進*/
-        Last_val=Line_Dire;
-        /* 初期化フラグ立てる */
-        LineFlag=1;
-    }
-
     /* どちらもラインに乗っていない状態 */
     if(In.Get(2)==0&&In.Get(3)==0) {
         if(Last_val==0) val_x-=10;
         if(Last_val==1) val_x+=10;
         else val_x+=0;
     }
-
     /* 左側のみ */
     if(In.Get(2)==1&&In.Get(3)==0) {
         val_x-=10;
@@ -565,14 +564,17 @@
     }
     /* どちらも乗っている */
     else
-        /* フラグ回収 */
-        LineFlag=0;
+        /* フラグを立てる */
+        LineFlag=1;
 
 
     SubOmuni(val_x,0,0);
 
     /* 到達 */
-    if(LineFlag==0) return 1;
+    if(LineFlag==1) {
+        LineFlag=0;
+        return 1;
+    }
     /* 未到達 */
     else return 0;
 }
\ No newline at end of file