![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
加減速走行時の次マーカ検出前のブレーキングシステムを導入。各速度域におけるブレーキのタイミングとPDパラメータの調整を入念に行うこと。また、LCDの表示事項はストップゴールの表示は必要性を感じなかったため、走行交差点数のパラメータ表示に変更した。
Dependencies: mbed AQM0802 CRotaryEncoder TB6612FNG
Diff: main.cpp
- Revision:
- 25:0dbd9ab9fd86
- Parent:
- 24:5fd9d128e587
- Child:
- 26:7d4150475983
--- a/main.cpp Sat Nov 23 01:59:23 2019 +0000 +++ b/main.cpp Sat Nov 23 02:41:50 2019 +0000 @@ -1,3 +1,8 @@ +//簡易コースを使って走行テストすること。 +//加減速走行時は1msごとの割込み処理の精度が重要になるので +//printf等の時間のかかる処理は控えるようにする。 + + ////ライントレースサンプル #include "mbed.h" #include "CRotaryEncoder.h" @@ -29,17 +34,19 @@ #define S_K2 2.4f //2倍 #define S_K3 4.7f //4倍 -//ラインセンサ各種制御成分 -#define S_KP 1.0f //ラインセンサ比例成分。大きいほど曲がりやすい -#define S_KD 0.5f //ラインセンサ微分成分。大きいほど急なラインずれに強くなる。 -#define S_KP1 0.5f -#define S_KP2 0.8f -#define S_KP3 1.2f +//ラインセンサ各種制御成分 +#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_DEFAULT_HIGH 1.0f -#define S_KD1 0.2f -#define S_KD2 0.4f -#define S_KD3 0.6f +#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 //////////☆★☆★☆★☆★☆★////////////// @@ -116,12 +123,12 @@ ////↓左右タイヤの走行距離格納。1走目のコース記憶処理に使用する。マーカ通過ごとにリセット。 long int Distance_memory_A=0, Distance_memory_B=0; -long int SSKP=0;////??? -long int SSKD=0;////??? +long int S_Kp=S_KP_DEFAULT;////センサP成分 +long int S_Kd=S_KD_DEFAULT;////センサD成分 long int Marker_Run_Distance=0; long int Speed_A=0, Speed_B=0; //機体の現在速度を格納 -long int default_speed = DEFAULT_SPEED; //1走目の標準速度 +long int Default_Speed = DEFAULT_SPEED; //1走目の標準速度 long int Low_Speed = LOW_SPEED; //2走目以降の低速 long int Medium_Speed = MEDIUM_SPEED; //2走目以降の中速 long int High_Speed = HIGH_SPEED; //2走目以降の高速 @@ -231,37 +238,38 @@ //左右タイヤの走行距離の差分を用いた曲率演算 if(((course_data[Row][0])>(course_data[Row][1]*1.8f)) || ((course_data[Row][1])>(course_data[Row][0]*1.8f))){//左が右より80%以上早いか20%以上遅いとき - Imaginary_Speed=Low_Speed;//急カーブは低速とする。 - SSKP=S_KP1;//ゲインを高めに設定 - SSKD=S_KD1; + 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%以上遅いとき Imaginary_Speed=Medium_Speed;//中カーブは中速とする。 - SSKP=S_KP2;//ゲインを中に設定 - SSKD=S_KD2; }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;//緩いカーブは高速とする。 - SSKP=S_KP3;//ゲインを低めに設定 - SSKD=S_KD3; }else{ Imaginary_Speed=High_Speed;//直線は高速とする。 - SSKP=S_KP;//ゲインを極低に設定 - SSKD=S_KD;// } course_data[Row][2]=Imaginary_Speed; //仮想の演算速度を格納 - - //モニタ表示 -// PC.printf("left:%.2f\t",course_data[Row][0]); -// PC.printf("right:%.2f\t",course_data[Row][1]); -// PC.printf("speed:%.2f\n\r",course_data[Row][2]); + } //2走目の加減速走行に使用。1走目で演算したマーカ間の仮想速度を機体目標速度に設定する。 -void second_speed_control(){ +void second_speed_control(){ + if(course_data[Row][2]==Default_Speed){//標準速度区間のとき + S_Kp=S_KP_DEFAULT; + S_Kd=S_KD_DEFAULT; + }else if(course_data[Row][2]==Low_Speed){//低速走行区間のとき + S_Kp=S_KP_LOW; + S_Kd=S_KD_LOW; + }else if(course_data[Row][2]==Medium_Speed){//中速走行区間のとき + S_Kp=S_KP_MEDIUM; + S_Kd=S_KD_MEDIUM; + }else if(course_data[Row][2]==High_Speed){//高速走行区間のとき + S_Kp=S_KP_HIGH; + S_Kd=S_KD_HIGH; + }else{// + S_Kp=S_KP_DEFAULT_HIGH; + S_Kd=S_KD_DEFAULT_HIGH; + } Target_Speed_A=course_data[Row][2];//記憶走行で演算した仮想速度を使う - Target_Speed_B=course_data[Row][2]; -// PC.printf("left:%.2f\t",course_data[Row][0]); -// PC.printf("right:%.2f\t",course_data[Row][1]); -// PC.printf("speed:%.2f\n\r",course_data[Row][2]); + Target_Speed_B=course_data[Row][2]; } @@ -354,8 +362,8 @@ All_Sensor_Data=-(S2_Data*S_K3+S3_Data*S_K2+S4_Data*S_K1)+(S5_Data*S_K1+S6_Data*S_K2+S7_Data*S_K3); Sensor_Diff[1]=Sensor_Diff[0];//過去のラインセンサ偏差を退避 Sensor_Diff[0]=All_Sensor_Data; - Sensor_P=All_Sensor_Data*SSKP; //ラインセンサ比例成分の演算 - Sensor_D=(Sensor_Diff[0]-Sensor_Diff[1])*SSKD; //ラインセンサ微分成分の演算 + Sensor_P=All_Sensor_Data*S_Kp; //ラインセンサ比例成分の演算 + Sensor_D=(Sensor_Diff[0]-Sensor_Diff[1])*S_Kd; //ラインセンサ微分成分の演算 Sensor_PD=Sensor_P+Sensor_D; ////モータ現在速度の取得 @@ -465,12 +473,13 @@ lcd.locate(0,1); lcd.print(Speed_Str); if(Sw==0){ - Medium_Speed=MEDIUM_SPEED+(Enc_B_Rotate/16);//標準速度調整 - sprintf(Speed_Str,"%04d",Medium_Speed);//速度情報文字列変換 + Default_Speed=DEFAULT_SPEED+(Enc_B_Rotate/16);//標準速度調整 + sprintf(Speed_Str,"%04d",Default_Speed);//速度情報文字列変換 } else if(Sw==1) {//2走目。加減速による高速走行 - High_Speed=HIGH_SPEED+(Enc_B_Rotate/16); - sprintf(Speed_Str,"%04d",High_Speed);//速度情報文字列変換 + Low_Speed=LOW_SPEED+(Enc_B_Rotate/16); + Default_Speed=DEFAULT_SPEED+(Enc_B_Rotate/16); + sprintf(Speed_Str,"%04d",Low_Speed);//速度情報文字列変換 } else if(Sw==2)//3走目。加減速による超高速走行 Default_High_Speed=DEFAULT_HIGH_SPEED+(Enc_B_Rotate/16); @@ -485,10 +494,25 @@ lcd.locate(0,0); lcd.print("GO!!"); wait(2); + if(Sw==0){ //1走目のとき + S_Kp=S_KP_DEFAULT; + S_Kd=S_KD_DEFAULT; + Target_Speed_A=Default_Speed; + Target_Speed_B=Default_Speed; + }else if(Sw==1){ //2走目のとき + S_Kp=S_KP_DEFAULT; + S_Kd=S_KD_DEFAULT; + Target_Speed_A=Default_Speed; + Target_Speed_B=Default_Speed; + }else{ //3走目以降のとき + S_Kp=S_KP_DEFAULT_HIGH; + S_Kd=S_KD_DEFAULT_HIGH; + Target_Speed_A=Default_High_Speed; + Target_Speed_B=Default_High_Speed; + } Stop_Distance=0; Machine_Status&=0x7F;//ストップ状態解除 - Target_Speed_A=Medium_Speed; - Target_Speed_B=Medium_Speed; + }else{//機体走行中であったとき