![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
ニュージェネの28日午前時点のベストチューニング。1走目900[mm/sec]、2走目850[mm/sec],3走目1000[mm/sec]で走行可能。バッテリは満充電8.4[V]で確認。
Dependencies: mbed AQM0802 CRotaryEncoder TB6612FNG
Diff: main.cpp
- Revision:
- 23:def04f2e894f
- Parent:
- 22:b40f7d0c0f12
- Child:
- 24:5fd9d128e587
--- a/main.cpp Fri Nov 22 04:20:20 2019 +0000 +++ b/main.cpp Fri Nov 22 10:52:35 2019 +0000 @@ -31,6 +31,14 @@ //ラインセンサ各種制御成分 #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_KD1 0.2f +#define S_KD2 0.4f +#define S_KD3 0.6f //////////☆★☆★☆★☆★☆★////////////// @@ -104,7 +112,10 @@ char MemoryA_Str[5]; //LCD閾値表示用文字列 char MemoryB_Str[5]; long int Distance_A=0,Distance_B=0; //タイヤ移動距離を格納[mm] -long int Distance_memory_A=0, Distance_memory_B=0; +long int Distance_memory_A=0, Distance_memory_B=0; + +long int SSKP=0; +long int SSKD=0; long int Marker_Run_Distance=0; long int Speed_A=0, Speed_B=0; //現在速度 @@ -137,7 +148,6 @@ int Row=0; //行変数 float course_data[100][3]; //記憶走行用配列 -float curvature=0; void sensor_analog_read(){ S1_Data=s1.read(); @@ -215,15 +225,23 @@ course_data[Row][1]=(float)Distance_memory_B; //キョクリツ演算 if((Distance_memory_A>(Distance_memory_B*1.8)) || (Distance_memory_A<(Distance_memory_B*0.2))){//左が右より80%以上早いか20%以上遅いとき - Imaginary_Speed=Low_Speed; + Imaginary_Speed=Low_Speed; + SSKP=S_KP1; + SSKD=S_KD1; }else if((Distance_memory_A>(Distance_memory_B*1.5)) || (Distance_memory_A<(Distance_memory_B*0.5))){//左が右より50%以上早いか50%以上遅いとき Imaginary_Speed=Medium_Speed; + SSKP=S_KP2; + SSKD=S_KD2; }else if((Distance_memory_A>(Distance_memory_B*1.2)) || (Distance_memory_A<(Distance_memory_A*0.8))){//左が右より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]); @@ -239,36 +257,7 @@ else{ Distance_memory_A=(Enc_Count_A*PULSE_TO_UM); Distance_memory_B=(Enc_Count_B*PULSE_TO_UM); - } - - /* - if((motor_a>(motor_b*1.8)) || (motor_a<(motor_b*0.2))){//左が右より80%以上早いか20%以上遅いとき - Imaginary_Speed=Low_Speed; - Target_Speed_A=Imaginary_Speed; - Target_Speed_B=Imaginary_Speed; - - }else if((motor_a>(motor_b*1.5)) || (motor_a<(motor_b*0.5))){//左が右より50%以上早いか50%以上遅いとき - Imaginary_Speed=Medium_Speed; - Target_Speed_A=Imaginary_Speed; - Target_Speed_B=Imaginary_Speed; - }else if((motor_a>(motor_b*1.2)) || (motor_a<(motor_b*0.8))){//左が右より20%以上早いか20%以上遅いとき⇒直線 - Imaginary_Speed=High_Speed; - Target_Speed_A=Imaginary_Speed; - Target_Speed_B=Imaginary_Speed; - } - else{ - Imaginary_Speed=High_Speed; - Target_Speed_A=Imaginary_Speed; - Target_Speed_B=Imaginary_Speed; - } -*/ - /*course_data[Row][0]=(float)Distance_memory_A; - course_data[Row][1]=(float)Distance_memory_B; - 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]);*/ - + } } //タイマ割り込み1[ms]周期 @@ -323,8 +312,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*S_KP; //ラインセンサ比例成分の演算 - Sensor_D=(Sensor_Diff[0]-Sensor_Diff[1])*S_KD; //ラインセンサ微分成分の演算 + Sensor_P=All_Sensor_Data*SSKP; //ラインセンサ比例成分の演算 + Sensor_D=(Sensor_Diff[0]-Sensor_Diff[1])*SSKD; //ラインセンサ微分成分の演算 Sensor_PD=Sensor_P+Sensor_D; ////モータ現在速度の取得