最終調整
Dependencies: mbed AQM0802 CRotaryEncoder TB6612FNG
Diff: main.cpp
- Revision:
- 26:7d4150475983
- Parent:
- 25:0dbd9ab9fd86
- Child:
- 27:9e234e087e70
--- 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){//機体停止状態の時