Ver1の改良

Dependencies:   mbed AQM0802 CRotaryEncoder TB6612FNG

Files at this revision

API Documentation at this revision

Comitter:
GGU
Date:
Sat Nov 23 07:09:32 2019 +0000
Parent:
25:0dbd9ab9fd86
Commit message:
test;

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r 0dbd9ab9fd86 -r 7d4150475983 main.cpp
--- a/main.cpp	Sat Nov 23 02:41:50 2019 +0000
+++ b/main.cpp	Sat Nov 23 07:09:32 2019 +0000
@@ -11,10 +11,10 @@
 
 
 //☆★☆★各種パラメータ調整箇所☆★☆★☆★
-#define     DEFAULT_SPEED       450     //1走目の基本速度
+#define     DEFAULT_SPEED       750     //1走目の基本速度
 #define     LOW_SPEED           450     //2走目の低速速度[mm/sec]
-#define     MEDIUM_SPEED        700     //2走目の低速速度[mm/sec]
-#define     HIGH_SPEED          800     //2走目の低速速度[mm/sec]
+#define     MEDIUM_SPEED        800     //2走目の低速速度[mm/sec]
+#define     HIGH_SPEED          1500     //2走目の低速速度[mm/sec]
 #define     DEFAULT_HIGH_SPEED  950     //3走目の低速速度[mm/sec]
 #define     STOP_DISTANCE   200000      //停止距離200000[um]⇒20[cm]
 #define     TURN_POWER      0.6         //コースアウト時の旋回力
@@ -26,8 +26,8 @@
                                         //コースの傷によってマーカ誤検知する場合は値を大きくする。
 #define     CROSS_JUDGE     4           //ラインセンサいくつ以上白線検知で交差点認識するか設定。
 //モータ速度のゲイン関連(むやみに調整しない)
-#define     M_KP            0.002f      //P(比例)制御成分
-#define     M_KD            0.001f      //D(微分)制御成分
+#define     M_KP            0.001f      //P(比例)制御成分
+#define     M_KD            0.0005f      //D(微分)制御成分
 
 //フォトリフレクタのゲイン(外側に行くにつれ値を何倍させたいか調整する。)
 #define     S_K1            1.0f        //float演算させる値には必ずfを付ける
@@ -37,16 +37,16 @@
 
 //ラインセンサ各種制御成分
 #define     S_KP_DEFAULT    1.0f        //ラインセンサ比例成分。大きいほど曲がりやすい
-#define     S_KP_HIGH       0.5f
-#define     S_KP_MEDIUM     0.8f
-#define     S_KP_LOW        1.2f
+#define     S_KP_HIGH       1.0f
+#define     S_KP_MEDIUM     1.0f
+#define     S_KP_LOW        1.0f
 #define     S_KP_DEFAULT_HIGH   1.0f
 
-#define     S_KD_DEFAULT    0.5f        //ラインセンサ微分成分。大きいほど急なラインずれに強くなる。
-#define     S_KD_HIGH       0.2f
-#define     S_KD_MEDIUM     0.4f
-#define     S_KD_LOW        0.6f
-#define     S_KD_DEFAULT_HIGH   0.5f
+#define     S_KD_DEFAULT    1.0f        //ラインセンサ微分成分。大きいほど急なラインずれに強くなる。
+#define     S_KD_HIGH       1.0f
+#define     S_KD_MEDIUM     1.0f
+#define     S_KD_LOW        1.0f
+#define     S_KD_DEFAULT_HIGH   1.0f
 //////////☆★☆★☆★☆★☆★//////////////
 
 
@@ -89,7 +89,7 @@
 */
 
 ///////////////////////////////////////  
-Serial      PC(USBTX,USBRX);
+//Serial      PC(USBTX,USBRX);
 CRotaryEncoder encoder_a(D1,D0);    //モータAのエンコーダ
 CRotaryEncoder encoder_b(D11,D12);  //モータBのエンコーダ
 Ticker      timer;                  //タイマ割込み用 
@@ -115,7 +115,7 @@
 long int    Enc_A_Rotate=0,Enc_B_Rotate=0;  //エンコーダパルス数。停止時の機体速度調整に使用。
 long int    Stop_Distance=STOP_DISTANCE;    //ストップ時の徐行→停止に使う
         /////↓エンコーダパルス数。コース記憶時のエンコーダパルスの蓄積に使用する。
-long int    Memory_Enc_Count_A=0,Memory_Enc_Count_B=0;   
+long long int    Memory_Enc_Count_A=0,Memory_Enc_Count_B=0;   
 char        MemoryA_Str[5];                 //LCD表示用。左モータの走行距離格納
 char        MemoryB_Str[5];                 //LCD表示用。右モータの走行距離格納
 long int    Distance_A=0,Distance_B=0;      //タイヤ移動距離を格納[mm]。
@@ -237,13 +237,14 @@
     course_data[Row][1]=(Memory_Enc_Count_B*PULSE_TO_UM);//右タイヤのマーカ間の走行距離を格納                
                                 
     //左右タイヤの走行距離の差分を用いた曲率演算
-    if(((course_data[Row][0])>(course_data[Row][1]*1.8f)) || ((course_data[Row][1])>(course_data[Row][0]*1.8f))){//左が右より80%以上早いか20%以上遅いとき
+    if(((course_data[Row][0])>(course_data[Row][1]*4.0f)) || ((course_data[Row][1])>(course_data[Row][0]*4.0f))){//左が右より80%以上早いか20%以上遅いとき
         Imaginary_Speed=Low_Speed;//急カーブは低速とする。    
-    }else if((course_data[Row][0]>(course_data[Row][1]*1.5f)) || (course_data[Row][1]>(course_data[Row][0]*1.5f))){//左が右より50%以上早いか50%以上遅いとき
+    }else if((course_data[Row][0]>(course_data[Row][1]*2.5f)) || (course_data[Row][1]>(course_data[Row][0]*2.5f))){//左が右より50%以上早いか50%以上遅いとき
         Imaginary_Speed=Medium_Speed;//中カーブは中速とする。
     }else if((course_data[Row][0]>(course_data[Row][1]*1.2f)) || (course_data[Row][1]>(course_data[Row][0]*1.2f))){//左が右より20%以上早いか20%以上遅いとき⇒直線
         Imaginary_Speed=High_Speed;//緩いカーブは高速とする。
-    }else{
+    }
+    else{
         Imaginary_Speed=High_Speed;//直線は高速とする。
     }                              
     course_data[Row][2]=Imaginary_Speed; //仮想の演算速度を格納
@@ -292,12 +293,14 @@
     //////コース記憶に使用するエンコーダパルス数の蓄積処理
     Memory_Enc_Count_A+=Enc_Count_A;
     Memory_Enc_Count_B+=Enc_Count_B;    
-    //////機体停止時の徐行処理に使用するエンコーダパルス数の蓄積
-    Enc_A_Rotate+=Enc_Count_A;
-    Enc_B_Rotate+=Enc_Count_B;
+
     
     //////機体停止時の徐行用の走行距離演算
     if(Machine_Status&STOP){//機体停止状態の時
+        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;        
         if(Enc_B_Rotate<-6400)Enc_B_Rotate=-6400;
@@ -314,9 +317,7 @@
         if((Corner_Flag==1)&&(SG_Flag==1));//交差点通過中。何もしない    
     }else Marker_Pass_Flag=0;//マーカ通過終了       
 
-    if(Marker_Pass_Flag==1){//マーカ通過中は通過距離情報を蓄積する。
-        Marker_Run_Distance+=(Distance_A+Distance_B)/2;
-    }   
+ 
     //マーカ通過後、マーカ種類判別    
     if((Old_Marker_Pass_Flag==1)&&(Marker_Pass_Flag==0)){//マーカ通過後
         if(Marker_Run_Distance>MARKER_WIDTH){   //マーカ幅がもっともらしいとき
@@ -324,16 +325,20 @@
             else if((SG_Flag==1)&&(SG_Cnt==0)){ //ゴールスタートマーカの時⇒1回目
                 SG_Cnt=1;
                 if(Sw==0){              //1走目のとき
+                    Memory_Enc_Count_A=0;
+                    Memory_Enc_Count_B=0;                   
                     corner_curvature(); //曲率に応じた速度推定処理                
                     Row++;
-                }else if(Sw==1){        //2走目のとき
-                    ++Row;//一つ先のコース情報を読む。                 
-                    second_speed_control();//記憶情報から目標速度を設定する処理                     
+                }else if(Sw==1){        //2走目のとき                
+                    ++Row;//一つ先のコース情報を読む                   
+                    second_speed_control();//記憶情報から目標速度を設定する処理                  
                 }else{//3走目は定速で高速走行
                     Target_Speed_A=Default_High_Speed;
                     Target_Speed_B=Default_High_Speed;
                 }
             }else if((SG_Flag==1)&&(SG_Cnt==1)){//ゴールスタートマーカの時⇒2回目
+                corner_curvature(); //曲率に応じた速度推定処理                
+                Row++;            
                 Machine_Status|=STOP;           //機体停止状態へ                
                 Row=0;   //コース記憶用配列の行情報[通過マーカ情報]の初期化 
                 SG_Cnt=0;//スタート・ゴールマーカ情報のリセット   
@@ -417,7 +422,10 @@
         motor_a=0;
         motor_b=0;        
    }
-       
+    if(Marker_Pass_Flag==1){//マーカ通過中は通過距離情報を蓄積する。
+        Marker_Run_Distance+=(Distance_A+Distance_B)/2;
+    }  
+           
     //割込み終了時の各種パラメータリセット処理 
     //エンコーダ関連情報のリセット
     encoder_a.Set(0);//エンコーダクラスのパルス数情報のリセット
@@ -435,8 +443,7 @@
     lcd.locate(0,0);
     lcd.print("STOP");
     lcd.locate(0,1);
-    sprintf(Speed_Str,"%04d",Medium_Speed);
-    lcd.print(Speed_Str);        
+    
     
     while(1){
         Old_Sw_Ptn=Sw_Ptn;  //過去のスイッチ入力情報を退避
@@ -470,8 +477,7 @@
             
             lcd.locate(0,1);
             lcd.print("      ");
-            lcd.locate(0,1);
-            lcd.print(Speed_Str);              
+                       
             if(Sw==0){
                 Default_Speed=DEFAULT_SPEED+(Enc_B_Rotate/16);//標準速度調整                
                 sprintf(Speed_Str,"%04d",Default_Speed);//速度情報文字列変換
@@ -481,11 +487,13 @@
                 Default_Speed=DEFAULT_SPEED+(Enc_B_Rotate/16);
                 sprintf(Speed_Str,"%04d",Low_Speed);//速度情報文字列変換
             }
-            else if(Sw==2)//3走目。加減速による超高速走行                                           
+            else if(Sw==2){//3走目。加減速による超高速走行                                           
                 Default_High_Speed=DEFAULT_HIGH_SPEED+(Enc_B_Rotate/16);
-                sprintf(Speed_Str,"%04d",Default_High_Speed);//速度情報文字列変換               
+                sprintf(Speed_Str,"%04d",Default_High_Speed);//速度情報文字列変換  
+            }             
         }
-        
+            lcd.locate(0,1);
+            lcd.print(Speed_Str);           
                  
         if((Old_Sw_Ptn==PULL)&&(Sw_Ptn==PUSH)){//スイッチが押された瞬間
             if(Machine_Status&STOP){//機体停止状態の時