![](/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:
- 22:b40f7d0c0f12
- Parent:
- 21:97cc65e61580
- Child:
- 23:def04f2e894f
--- a/main.cpp Wed Nov 20 04:41:18 2019 +0000 +++ b/main.cpp Fri Nov 22 04:20:20 2019 +0000 @@ -6,9 +6,10 @@ //☆★☆★各種パラメータ調整箇所☆★☆★☆★ -#define DEFAULT_SPEED 300 //1走目の基本速度[mm/sec] -#define DEFAULT_SPEED1 800 -#define DEFAULT_SPEED2 900 +#define DEFAULT_SPEED 450 //1走目の基本速度[mm/sec] +#define DEFAULT_SPEED1 700 +#define DEFAULT_SPEED2 950 +#define DEFAULT_SPEED3 800 #define STOP_DISTANCE 200000 //停止距離200000[um]⇒20[cm] #define TURN_POWER 0.6 //コースアウト時の旋回力 #define PULSE_TO_UM 28 //エンコーダ1パルス当たりのタイヤ移動距離[um] @@ -50,7 +51,7 @@ //デジタル入力オブジェクト定義 DigitalIn push_sw(D13); /////アナログ入力オブジェクト定義////////// -/* + AnalogIn s1(D3); AnalogIn s2(A6); AnalogIn s3(A5); @@ -59,8 +60,8 @@ AnalogIn s6(A2); AnalogIn s7(A1); AnalogIn s8(A0); -*/ +/* AnalogIn s1(A1); AnalogIn s2(D3); AnalogIn s3(A6); @@ -69,7 +70,7 @@ AnalogIn s6(A3); AnalogIn s7(A2); AnalogIn s8(A0); - +*/ /////////////////////////////////////// Serial PC(USBTX,USBRX); @@ -111,9 +112,9 @@ long int Low_Speed=DEFAULT_SPEED; long int Medium_Speed=DEFAULT_SPEED1; long int High_Speed=DEFAULT_SPEED2; +long int High1_Speed=DEFAULT_SPEED3; char Speed_Str[5]; //LCD速度表示用文字列 long int Target_Speed_A=0,Target_Speed_B=0; //目標速度 -long int Target_Speed_A1=0,Target_Speed_B1=0; //目標速度 long int Motor_A_Diff[2]={0,0}; //過去の速度偏差と現在の速度偏差を格納 long int Motor_B_Diff[2]={0,0}; // float Motor_A_P,Motor_B_P; //モータ速度制御P成分 @@ -205,7 +206,43 @@ } } void coner_curvature(){ - if((motor_a>(motor_b*1.8)) || (motor_a<(motor_b*0.2))){//左が右より80%以上早いか20%以上遅いとき + if(Sw==0){ + Enc_Count_A=encoder_a.Get(); //エンコーダパルス数を取得 + Enc_Count_B=-encoder_b.Get(); + Distance_memory_A=(Enc_Count_A*PULSE_TO_UM); + Distance_memory_B=(Enc_Count_B*PULSE_TO_UM); + course_data[Row][0]=(float)Distance_memory_A; + 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; + }else if((Distance_memory_A>(Distance_memory_B*1.5)) || (Distance_memory_A<(Distance_memory_B*0.5))){//左が右より50%以上早いか50%以上遅いとき + Imaginary_Speed=Medium_Speed; + }else if((Distance_memory_A>(Distance_memory_B*1.2)) || (Distance_memory_A<(Distance_memory_A*0.8))){//左が右より20%以上早いか20%以上遅いとき⇒直線 + Imaginary_Speed=High_Speed; + } + else{ + Imaginary_Speed=High_Speed; + } + 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]); + } + else if(Sw==1){ + 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]); + } + 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; @@ -221,15 +258,16 @@ } else{ Imaginary_Speed=High_Speed; - Target_Speed_A=Imaginary_Speed; + Target_Speed_A=Imaginary_Speed; Target_Speed_B=Imaginary_Speed; } - course_data[Row][0]=(float)Distance_memory_A; +*/ + /*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]); + PC.printf("speed:%.2f\n\r",course_data[Row][2]);*/ } @@ -262,27 +300,15 @@ else if((SG_Flag==1)&&(SG_Cnt==0)){//ゴールスタートマーカの時⇒1回目 SG_Cnt=1; }else if((SG_Flag==1)&&(SG_Cnt==1)){//ゴールスタートマーカの時⇒2回目 - Machine_Status|=STOP; //機体停止状態へ - SG_Cnt=0; + Machine_Status|=STOP; //機体停止状態へ + Row=0; + SG_Cnt=0; }else if(Corner_Flag==1){//コーナマーカの時 Distance_memory_A=0; Distance_memory_B=0; - Enc_Count_A=encoder_a.Get(); //エンコーダパルス数を取得 - Enc_Count_B=-encoder_b.Get(); - Distance_memory_A=(Enc_Count_A*PULSE_TO_UM); - Distance_memory_B=(Enc_Count_B*PULSE_TO_UM); - - /*course_data[Row][0]=Distance_memory_A; - course_data[Row][1]=Distance_memory_B; - course_data[Row][2]=Imaginary_Speed; - - PC.printf("左:%.2f",course_data[Row][0]); - PC.printf("右:%.2f",course_data[Row][1]); - PC.printf("速度:%.2f",course_data[Row][2]); - */ + Coner_c++; coner_curvature(); - Row++; - + Row++; } }else{//マーカではなく、誤検知だった場合。 //何もしない @@ -347,16 +373,17 @@ Motor_A_Diff[0]=(Target_Speed_A-Speed_A); Motor_B_Diff[0]=(Target_Speed_B-Speed_B); } - else { - Target_Speed_A1=High_Speed; - Target_Speed_B1=High_Speed; + else if(Sw==1) { + Target_Speed_A=High_Speed; + Target_Speed_B=High_Speed; Motor_A_Diff[1]=Motor_A_Diff[0]; Motor_B_Diff[1]=Motor_B_Diff[0]; - Motor_A_Diff[0]=(Target_Speed_A1-Speed_A); - Motor_B_Diff[0]=(Target_Speed_B1-Speed_B); + Motor_A_Diff[0]=(Target_Speed_A-Speed_A); + Motor_B_Diff[0]=(Target_Speed_B-Speed_B); } + /* /////モータの速度制御 @@ -439,9 +466,9 @@ //lcd.print(MemoryB_Str); wait(5); Gray=DEFAULT_GRAY; - Medium_Speed=DEFAULT_SPEED1; - + Medium_Speed=DEFAULT_SPEED1; Sw++; + Coner_c=0; } if(Machine_Status&STOP){//機体停止状態の時 @@ -462,10 +489,14 @@ if(Sw==0){ Medium_Speed=DEFAULT_SPEED1+(Enc_B_Rotate/16);//標準速度調整 sprintf(Speed_Str,"%04d",Medium_Speed);//速度情報文字列変換 - }else { + } + else if(Sw==1) { High_Speed=DEFAULT_SPEED2+(Enc_B_Rotate/16); sprintf(Speed_Str,"%04d",High_Speed);//速度情報文字列変換 - } + } + else if(Sw==2) + High1_Speed=DEFAULT_SPEED3+(Enc_B_Rotate/16); + sprintf(Speed_Str,"%04d",High1_Speed);//速度情報文字列変換 }