Diff: main.cpp

Revision:

14:584e778a20ea

Parent:

13:97be8e29ae50

--- a/main.cpp	Thu Aug 29 01:07:15 2019 +0000
+++ b/main.cpp	Sun Sep 01 06:10:26 2019 +0000
@@ -6,13 +6,14 @@
 
 
 //☆★☆★各種パラメータ調整箇所☆★☆★☆★
-#define     DEFAULT_SPEED   800     //1走目の基本速度[mm/sec]
-#define     TURN_POWER      0.47     //コースアウト時の旋回力
+#define     DEFAULT_SPEED   700     //1走目の基本速度[mm/sec]
+#define     STOP_DISTANCE   200000  //停止距離200000[um]⇒20[cm]
+#define     TURN_POWER      0.3     //コースアウト時の旋回力
 #define     PULSE_TO_UM     30      //エンコーダ1パルス当たりのタイヤ移動距離[um]
 #define     INTERRUPT_TIME  1000    //割りこみ周期[us]
-#define     DEFAULT_GRAY    0.5f    //フォトリフレクタデジタル入力の閾値
+#define     DEFAULT_GRAY    0.3f    //フォトリフレクタデジタル入力の閾値
                                     //シリアル通信でSensor_Digital値を確認し調整する。
-#define     MARKER_WIDTH    15      //マーカ幅[mm]（ビニルテープ幅19[mm]以内）
+#define     MARKER_WIDTH    13000      //マーカ幅[um]（ビニルテープ幅19[um]以内）
                                     //コースの傷によってマーカ誤検知する場合は値を大きくする。
 #define     CROSS_JUDGE     4       //ラインセンサいくつ以上白線検知で交差点認識するか設定。
 //モータ速度のゲイン関連(むやみに調整しない)
@@ -22,11 +23,11 @@
 //フォトリフレクタのゲイン（外側に行くにつれ値を何倍させたいか調整する。）
 #define     S_K1            1.0f    //float演算させる値には必ずfを付ける
 #define     S_K2            2.0f    //2倍
-#define     S_K3            4.0f    //4倍
+#define     S_K3            3.0f    //4倍
 
 //ラインセンサ各種制御成分
-#define     S_KP            1.07f    //ラインセンサ比例成分。大きいほど曲がりやすい
-#define     S_KD            0.7f    //ラインセンサ微分成分。大きいほど急なラインずれに強くなる。
+#define     S_KP            0.5f    //ラインセンサ比例成分。大きいほど曲がりやすい
+#define     S_KD            0.5f    //ラインセンサ微分成分。大きいほど急なラインずれに強くなる。
 //////////☆★☆★☆★☆★☆★//////////////
 
 
@@ -77,7 +78,7 @@
 float   Gray=DEFAULT_GRAY;
 long int Enc_Count_A=0,Enc_Count_B=0;        //エンコーダパルス数を格納
 long int Enc_A_Rotate=0,Enc_B_Rotate=0;
-
+long int Stop_Distance=STOP_DISTANCE;
 long int Distance_A=0,Distance_B=0;          //タイヤ移動距離を格納[mm]
 long int Marker_Run_Distance=0; 
 long int Speed_A=0,  Speed_B=0;              //現在速度
@@ -178,17 +179,14 @@
     
     //機体状態の取得
     Machine_Status_Set();
-
-    //交差点の認識
-    if(Sensor_Cnt>=CROSS_JUDGE )Cross_Flag=1;//ラインセンサ4つ以上検知状態の時は交差点を示す。
-    
+  
     //各種マーカの検知
     Old_Marker_Pass_Flag=Marker_Pass_Flag;//過去のフラグを退避
     if(Sensor_Digital&0x81){                   //マーカセンサ検知時
         Marker_Pass_Flag=1;                     //マーカ通過中フラグをON
         if(Sensor_Digital&0x80)Corner_Flag=1;  //コーナセンサの検知
         if(Sensor_Digital&0x01)SG_Flag=1;      //スタートゴールセンサの検知
-        if((Corner_Flag==1)&&(SG_Flag==1));//交差点通過中。何もしない    
+        if((Corner_Flag==1)&&(SG_Flag==1))Cross_Flag=1;//交差点通過中。何もしない    
     }else Marker_Pass_Flag=0;//マーカ通過終了       
 
     //マーカ通過後、マーカ種類判別    
@@ -224,22 +222,29 @@
     ////モータ現在速度の取得            
     Enc_Count_A=encoder_a.Get();   //エンコーダパルス数を取得
     Enc_Count_B=-encoder_b.Get();
-    Distance_A=(Enc_Count_A*PULSE_TO_UM);  //移動距離をmm単位で格納
+    Distance_A=(Enc_Count_A*PULSE_TO_UM);  //移動距離をum単位で格納
     Distance_B=(Enc_Count_B*PULSE_TO_UM);
     Speed_A=(Distance_A*1000)/INTERRUPT_TIME;//走行速度演算[mm/s]
     Speed_B=(Distance_B*1000)/INTERRUPT_TIME;
     
     if(Machine_Status&STOP){//機体停止状態の時
-        Enc_A_Rotate+=Enc_Count_A;//閾値用に左エンコーダ値の蓄積
+        if(Stop_Distance>=STOP_DISTANCE){
+         Enc_A_Rotate+=Enc_Count_A;//閾値用に左エンコーダ値の蓄積 
+         Enc_B_Rotate+=Enc_Count_B;//速度用に右エンコーダ値の蓄積                   
+        }
         if(Enc_A_Rotate<-6400)Enc_A_Rotate=-6400;
         if(Enc_A_Rotate>6400)Enc_A_Rotate=6400;        
-        Enc_B_Rotate+=Enc_Count_B;//速度用に右エンコーダ値の蓄積
         if(Enc_B_Rotate<-6400)Enc_B_Rotate=-6400;
         if(Enc_B_Rotate>6400)Enc_B_Rotate=6400;
+        if((Distance_A>=0)&&(Distance_B>=0))Stop_Distance+=(Distance_A+Distance_B)/2;
+        if(Stop_Distance<0)Stop_Distance=0;
+        if(Stop_Distance>STOP_DISTANCE)Stop_Distance=STOP_DISTANCE;
     }
     if(Marker_Pass_Flag==1){//マーカ通過中は通過距離情報を蓄積する。
         Marker_Run_Distance+=(Distance_A+Distance_B)/2;
     }
+    
+    
     //エンコーダ関連情報のリセット
     Distance_A=0;
     Distance_B=0;
@@ -284,7 +289,7 @@
         }else if(Machine_Status&RUN_COURSE_ROUT){   //右端センサ振り切れた時
             motor_a=-(TURN_POWER);  //右旋回    
             motor_b=-(-TURN_POWER);
-        }else if(Cross_Flag==1){//交差点通過中
+        }else if(Sensor_Cnt>=CROSS_JUDGE){//交差点通過中
             motor_a=-0.3;//交差点なので控えめの速度で直進
             motor_b=-0.3;
         }
@@ -293,8 +298,14 @@
             motor_b=-Motor_B_Pwm;
         }
     }else{//停止状態の時はモータへの出力は無効
-        motor_a=0;
-        motor_b=0;        
+        if(Stop_Distance<STOP_DISTANCE){
+            motor_a=-Motor_A_Pwm/4;
+            motor_b=-Motor_B_Pwm/4;
+
+        }else{
+            motor_a=0;
+            motor_b=0;
+        }        
     }                       
 }
 
@@ -311,11 +322,15 @@
     while(1){
         Old_Sw_Ptn=Sw_Ptn;  //過去のスイッチ入力情報を退避
         Sw_Ptn=push_sw;     //現在のスイッチ入力情報の取得
-        if((!(Old_Machine_Status&=STOP))&&(Machine_Status&=STOP)){//走行終了時
+        PC.printf("Sensor_Digital:%02x\tMachine_Status:%02x\tSG_Cnt:%d\r\n",Sensor_Digital,Machine_Status,SG_Cnt);
+        if((!(Old_Machine_Status&STOP))&&(Machine_Status&STOP)){//走行終了時
             lcd.locate(0,0);
             lcd.print("     ");
             lcd.locate(0,0);
-            lcd.print("STOP");            
+            lcd.print("STOP");
+            wait(5);
+            Gray=DEFAULT_GRAY;
+            Machine_Speed=DEFAULT_SPEED;                            
         }
         if(Machine_Status&STOP){//機体停止状態の時
           
@@ -341,6 +356,7 @@
                 lcd.locate(0,0);
                 lcd.print("GO!!");
                 wait(2);
+                Stop_Distance=0;
                 Machine_Status&=0x7F;//ストップ状態解除
             }else{//機体走行中であったとき
                 //各種フラグのクリア