![](/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:
- 19:edf765724d2d
- Parent:
- 18:992846cd1d5d
- Child:
- 20:3b35311f4576
--- a/main.cpp Mon Nov 18 05:36:49 2019 +0000 +++ b/main.cpp Mon Nov 18 11:51:49 2019 +0000 @@ -129,9 +129,12 @@ int SG_Flag=0; int SG_Cnt=0; int Cross_Flag=0; +long int Imaginary_Speed=0; + int Row=0; //行変数 float course_data[100][3]; //記憶走行用配列 +float curvature=0; void sensor_analog_read(){ S1_Data=s1.read(); @@ -199,7 +202,34 @@ Machine_Status &= 0xFB;//右コースアウト情報のみマスク } } - +void coner_curvature(){ + 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]周期 void timer_interrupt(){ @@ -232,8 +262,7 @@ }else if((SG_Flag==1)&&(SG_Cnt==1)){//ゴールスタートマーカの時⇒2回目 Machine_Status|=STOP; //機体停止状態へ SG_Cnt=0; - }else if(Corner_Flag==1){//コーナマーカの時 - + }else if(Corner_Flag==1){//コーナマーカの時 Distance_memory_A=0; Distance_memory_B=0; Enc_Count_A=encoder_a.Get(); //エンコーダパルス数を取得 @@ -241,15 +270,15 @@ 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][0]=Distance_memory_A; course_data[Row][1]=Distance_memory_B; - course_data[Row][2]=Medium_Speed; + course_data[Row][2]=Imaginary_Speed; - /*PC.printf("左:%.2f",course_data[Row][0]); + PC.printf("左:%.2f",course_data[Row][0]); PC.printf("右:%.2f",course_data[Row][1]); PC.printf("速度:%.2f",course_data[Row][2]); */ - + coner_curvature(); Row++; } @@ -328,7 +357,8 @@ Motor_A_Diff[0]=(Target_Speed_A1-Speed_A); Motor_B_Diff[0]=(Target_Speed_B1-Speed_B); } - /* + +/* /////モータの速度制御 //過去の速度偏差を退避 Motor_A_Diff[1]=Motor_A_Diff[0]; @@ -341,6 +371,7 @@ Motor_B_Diff[0]=(Target_Speed_B1-Speed_B); */ + //P成分演算 Motor_A_P=Motor_A_Diff[0]*M_KP; Motor_B_P=Motor_B_Diff[0]*M_KP; @@ -359,7 +390,7 @@ else if(Motor_A_Pwm<-0.95)Motor_A_Pwm=-0.95f; if(Motor_B_Pwm>0.95f)Motor_B_Pwm=0.95f; else if(Motor_B_Pwm<-0.95f)Motor_B_Pwm=-0.95f; - + /* //モータへの出力 if(!(Machine_Status&STOP)){//マシンが停止状態でなければ if(Machine_Status&RUN_COURSE_LOUT){ //左端センサ振り切れた時 @@ -379,9 +410,12 @@ }else{//停止状態の時はモータへの出力は無効 motor_a=0; motor_b=0; - } + }*/ } + + + int main() { timer.attach_us(&timer_interrupt,INTERRUPT_TIME);//タイマ割り込みスタート lcd.cls();//表示クリア