![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
玉田機体の調整
Dependencies: mbed AQM0802 CRotaryEncoder TB6612FNG
Diff: main.cpp
- Revision:
- 7:cfbf8d4a4d36
- Parent:
- 6:afd8f0d02c8d
- Child:
- 8:15b4e1d7a2c5
diff -r afd8f0d02c8d -r cfbf8d4a4d36 main.cpp --- a/main.cpp Tue Aug 27 05:37:18 2019 +0000 +++ b/main.cpp Wed Aug 28 00:13:57 2019 +0000 @@ -1,10 +1,6 @@ -////ライントレースサンプルver3 -///モータ速度制御プログラムを追加。 -///ラインセンサPD制御を追加。 -///M_KP,M_KDは別途straight_speed_controlのプログラムで検証した値をセットする。 -///フォトリフレクタのゲイン、ラインセンサの各種成分の調整を行い、 -///ラインをきれいにトレースできるよう挑戦する。 - +////ライントレースサンプルver5 +///マーカ検知機能の追加 +///スタートマーカ検知後、ゴールマーカの検知で停止するプログラム #include "mbed.h" #include "CRotaryEncoder.h" #include "TB6612.h" @@ -16,6 +12,10 @@ #define PULSE_TO_UM 30 //エンコーダ1パルス当たりのタイヤ移動距離[um] #define INTERRUPT_TIME 1000 //割りこみ周期[us] #define GRAY 0.2f //フォトリフレクタデジタル入力の閾値 + //シリアル通信でSensor_Digital値を確認し調整する。 +#define MARKER_WIDTH 10 //マーカ幅[mm](ビニルテープ幅19[mm]以内) + //コースの傷によってマーカ誤検知する場合は値を大きくする。 +#define CROSS_JUDGE 4 //ラインセンサいくつ以上白線検知で交差点認識するか設定。 //モータ速度のゲイン関連(むやみに調整しない) #define M_KP 0.002f //P(比例)制御成分 #define M_KD 0.001f //D(微分)制御成分 @@ -28,8 +28,6 @@ //ラインセンサ各種制御成分 #define S_KP 0.5f //ラインセンサ比例成分。大きいほど曲がりやすい #define S_KD 0.3f //ラインセンサ微分成分。大きいほど急なラインずれに強くなる。 - - //////////☆★☆★☆★☆★☆★////////////// //機体状態の定義 @@ -67,7 +65,8 @@ float Sensor_D=0.0f; //ラインセンサD(微分成分)制御量 float Sensor_PD=0.0f; //ラインセンサP,D成分の合計 long int Enc_Count_A=0,Enc_Count_B=0; //エンコーダパルス数を格納 -long int Distance_A=0,Distance_B=0; //タイヤ移動距離を格納[mm] +long int Distance_A=0,Distance_B=0; //タイヤ移動距離を格納[mm] +long int Marker_Run_Distance=0; long int Speed_A=0, Speed_B=0; //現在速度 long int Target_Speed_A=0,Target_Speed_B=0; //目標速度 long int Motor_A_Diff[2]={0,0}; //過去の偏差と現在の偏差を格納 @@ -77,10 +76,15 @@ float Motor_A_Pwm,Motor_B_Pwm; //モータへの出力 unsigned char Sensor_Digital =0x00; unsigned char Old_Sensor_Digital=0x00; +int Sensor_Cnt=0; unsigned char Machine_Status =0x00; //機体状態 unsigned char Old_Machine_Status=0x00; //過去の機体状態 - - +int Marker_Pass_Flag = 0; +int Old_Marker_Pass_Flag=0; +int Corner_Flag=0; +int SG_Flag=0; +int SG_Cnt=0; +int Cross_Flag=0; void sensor_analog_read(){ S1_Data=s1.read(); S2_Data=s2.read(); @@ -91,28 +95,46 @@ S7_Data=s7.read(); S8_Data=s8.read(); } + + void sensor_digital_read(){//8つのフォトリフレクタの入力を8ビットのデジタルパターンに変換 + Sensor_Cnt=0; Old_Sensor_Digital=Sensor_Digital; - if(S1_Data>GRAY)Sensor_Digital |= 0x80; //7ビット目のみセット (1にする。) - else Sensor_Digital &= 0x7F; //7ビット目のみマスク(0にする。) - if(S2_Data>GRAY)Sensor_Digital |= 0x40; //6ビット目のみセット (1にする。) - else Sensor_Digital &= 0xBF; //6ビット目のみマスク(0にする。) - if(S3_Data>GRAY)Sensor_Digital |= 0x20; //5ビット目のみセット (1にする。) - else Sensor_Digital &= 0xDF; //5ビット目のみマスク(0にする。) - if(S4_Data>GRAY)Sensor_Digital |= 0x10; //4ビット目のみセット (1にする。) - else Sensor_Digital &= 0xEF; //4ビット目のみマスク(0にする。) - if(S5_Data>GRAY)Sensor_Digital |= 0x08; //3ビット目のみセット (1にする。) - else Sensor_Digital &= 0xF7; //3ビット目のみマスク(0にする。) - if(S6_Data>GRAY)Sensor_Digital |= 0x04; //2ビット目のみセット (1にする。) - else Sensor_Digital &= 0xFB; //2ビット目のみマスク(0にする。) - if(S7_Data>GRAY)Sensor_Digital |= 0x02; //1ビット目のみセット (1にする。) - else Sensor_Digital &= 0xFD; //1ビット目のみマスク(0にする。) - if(S8_Data>GRAY)Sensor_Digital |= 0x01; //0ビット目のみセット (1にする。) - else Sensor_Digital &= 0xFE; //0ビット目のみマスク(0にする。) + if(S1_Data>GRAY){ + Sensor_Digital |= 0x80; //7ビット目のみセット (1にする。) + }else Sensor_Digital &= 0x7F; //7ビット目のみマスク(0にする。) + if(S2_Data>GRAY){ + Sensor_Digital |= 0x40; //6ビット目のみセット (1にする。) + Sensor_Cnt++; + }else Sensor_Digital &= 0xBF; //6ビット目のみマスク(0にする。) + if(S3_Data>GRAY){ + Sensor_Digital |= 0x20; //5ビット目のみセット (1にする。) + Sensor_Cnt++; + }else Sensor_Digital &= 0xDF; //5ビット目のみマスク(0にする。) + if(S4_Data>GRAY){ + Sensor_Digital |= 0x10; //4ビット目のみセット (1にする。) + Sensor_Cnt++; + }else Sensor_Digital &= 0xEF; //4ビット目のみマスク(0にする。) + if(S5_Data>GRAY){ + Sensor_Digital |= 0x08; //3ビット目のみセット (1にする。) + Sensor_Cnt++; + }else Sensor_Digital &= 0xF7; //3ビット目のみマスク(0にする。) + if(S6_Data>GRAY){ + Sensor_Digital |= 0x04; //2ビット目のみセット (1にする。) + Sensor_Cnt++; + }else Sensor_Digital &= 0xFB; //2ビット目のみマスク(0にする。) + if(S7_Data>GRAY){ + Sensor_Digital |= 0x02; //1ビット目のみセット (1にする。) + Sensor_Cnt++; + }else Sensor_Digital &= 0xFD; //1ビット目のみマスク(0にする。) + if(S8_Data>GRAY){ + Sensor_Digital |= 0x01; //0ビット目のみセット (1にする。) + }else Sensor_Digital &= 0xFE; //0ビット目のみマスク(0にする。) } void Machine_Status_Set(){ - Old_Machine_Status=Machine_Status; + Old_Machine_Status=Machine_Status; + //機体がライン中央に位置するとき if(Sensor_Digital&RUN_COURSE_CENTER )Machine_Status|=RUN_COURSE_CENTER; else Machine_Status &= 0xE7;//ライン中央情報のマスク @@ -131,10 +153,46 @@ } void timer_interrupt(){ + //ラインセンサ情報取得 sensor_analog_read(); sensor_digital_read(); + + //機体状態の取得 Machine_Status_Set(); + + //交差点の認識 + if(Sensor_Cnt>=CROSS_JUDGE )Cross_Flag=1;//ラインセンサ4つ以上検知状態の時は交差点を示す。 + + //各種マーカの検知 + Old_Marker_Pass_Flag=Marker_Pass_Flag;//過去のフラグを退避 + if(Sensor_Digital&0x81){ //マーカセンサ検知時 + Marker_Pass_Flag=1; //マーカ通過中フラグをON + if(Sensor_Digital&0x80)Corner_Flag=1; //コーナセンサの検知 + if(Sensor_Digital&0x01)SG_Flag=1; //スタートゴールセンサの検知 + if((Corner_Flag==1)&&(SG_Flag==1));//交差点通過中。何もしない + }else Marker_Pass_Flag=0;//マーカ通過終了 + + //マーカ通過後、マーカ種類判別 + if((Old_Marker_Pass_Flag==1)&&(Marker_Pass_Flag==0)){//マーカ通過後 + if(Marker_Run_Distance>MARKER_WIDTH){//マーカ幅がもっともらしいとき + if(Cross_Flag==1);//交差点の時は何もしない + 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; + }else if(Corner_Flag==1){//コーナマーカの時 + //地区大会では何もしない + } + }else{//マーカではなく、誤検知だった場合。 + //何もしない + } + Corner_Flag=0; + SG_Flag=0; + Cross_Flag=0; + Marker_Run_Distance=0;//マーカ通過距離情報リセット + } //センサ取得値の重ね合わせ(端のセンサほどモータ制御量を大きくする) 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); @@ -152,8 +210,12 @@ Distance_B=(Enc_Count_B*PULSE_TO_UM); Speed_A=(Distance_A*1000)/INTERRUPT_TIME;//走行速度演算[mm/s] Speed_B=(Distance_B*1000)/INTERRUPT_TIME; + if(Marker_Pass_Flag==1){//マーカ通過中は通過距離情報を蓄積する。 + Marker_Run_Distance+=(Distance_A+Distance_B)/2; + } Distance_A=0; Distance_B=0; + encoder_a.Set(0); encoder_b.Set(0); @@ -191,7 +253,11 @@ }else if(Machine_Status&RUN_COURSE_ROUT){ motor_a=-(TURN_POWER); motor_b=-(-TURN_POWER); - }else{ + }else if(Cross_Flag==1){//交差点通過中 + motor_a=-0.3;//交差点なので控えめの速度で直進 + motor_b=-0.3; + } + else{ motor_a=-Motor_A_Pwm; motor_b=-Motor_B_Pwm; } @@ -205,8 +271,9 @@ timer.attach_us(&timer_interrupt,INTERRUPT_TIME);//タイマ割り込みスタート while(1){ wait(1); - PC.printf("Sensor_Digital:0x%02x\t Old_Sensor_Digital:0x%02x\t Machine_Status:0x%02x\t Old_Machine_Status:0x%02x\r\n"\ - ,Sensor_Digital,Old_Sensor_Digital\ - ,Machine_Status,Old_Machine_Status);//表示 + //センサ検知情報、機体情報の表示 + PC.printf("Sensor_Digital:0x%02x\t Machine_Status:0x%02x\t",Sensor_Digital,Machine_Status); + //スタートゴールマーカ検知回数の表示 + PC.printf("SG_Cnt:%d\r\n",SG_Cnt); } } \ No newline at end of file