ライントレースサンプルプログラム。左モータにてデジタル閾値GRAYの調整を可能とした。GRAY調整により、旋回の反応性・マーカの判別精度に変化が見られるか検証すること。
Dependencies: mbed AQM0802 CRotaryEncoder TB6612FNG
main.cpp
00001 ////ライントレースサンプルver7_1 00002 #include "mbed.h" 00003 #include "CRotaryEncoder.h" 00004 #include "TB6612.h" 00005 #include "AQM0802.h" 00006 00007 00008 //☆★☆★各種パラメータ調整箇所☆★☆★☆★ 00009 #define DEFAULT_SPEED 950 //1走目の基本速度[mm/sec] 00010 #define TURN_POWER 0.5 //コースアウト時の旋回力 00011 #define PULSE_TO_UM 28 //エンコーダ1パルス当たりのタイヤ移動距離[um] 00012 #define INTERRUPT_TIME 1000 //割りこみ周期[us] 00013 #define DEFAULT_GRAY 0.5f //フォトリフレクタデジタル入力の閾値 00014 //シリアル通信でSensor_Digital値を確認し調整する。 00015 #define MARKER_WIDTH 11000 //マーカ幅[um](ビニルテープ幅19000[um]以内) 00016 //コースの傷によってマーカ誤検知する場合は値を大きくする。 00017 #define CROSS_JUDGE 4 //ラインセンサいくつ以上白線検知で交差点認識するか設定。 00018 //モータ速度のゲイン関連(むやみに調整しない) 00019 #define M_KP 0.002f //P(比例)制御成分 00020 #define M_KD 0.001f //D(微分)制御成分 00021 00022 //フォトリフレクタのゲイン(外側に行くにつれ値を何倍させたいか調整する。) 00023 #define S_K1 1.0f //float演算させる値には必ずfを付ける 00024 #define S_K2 2.4f //2倍 00025 #define S_K3 4.7f //4倍 00026 00027 //ラインセンサ各種制御成分 00028 #define S_KP 1.2f //ラインセンサ比例成分。大きいほど曲がりやすい 00029 #define S_KD 0.5f //ラインセンサ微分成分。大きいほど急なラインずれに強くなる。 00030 //////////☆★☆★☆★☆★☆★////////////// 00031 00032 00033 00034 //スイッチ状態の定義 00035 #define PUSH 0 //スイッチ押したときの状態 00036 #define PULL 1 //スイッチ離したときの状態 00037 //機体状態の定義 00038 #define STOP 0x80 //機体停止状態 00039 #define RUN_START 0x40 //スタートマーカ通過 00040 #define RUN_COURSE_LOUT 0x20 //左コースアウト状態 00041 #define RUN_COURSE_CENTER 0x18 //ライン中央走行状態 00042 #define RUN_COURSE_ROUT 0x04 //右コースアウト状態 00043 #define SECOND_RUN 0x02 //機体停止状態 00044 #define TUARD_RUN 0x01 //機体設定モード 00045 00046 00047 //デジタル入力オブジェクト定義 00048 DigitalIn push_sw(D13); 00049 /////アナログ入力オブジェクト定義////////// 00050 AnalogIn s1(D3); 00051 AnalogIn s2(A6); 00052 AnalogIn s3(A5); 00053 AnalogIn s4(A4); 00054 AnalogIn s5(A3); 00055 AnalogIn s6(A2); 00056 AnalogIn s7(A1); 00057 AnalogIn s8(A0); 00058 /////////////////////////////////////// 00059 Serial PC(USBTX,USBRX); 00060 CRotaryEncoder encoder_a(D1,D0); //モータAのエンコーダ 00061 CRotaryEncoder encoder_b(D11,D12); //モータBのエンコーダ 00062 Ticker timer; //タイマ割込み用 00063 TB6612 motor_a(D2,D7,D6); //モータA制御用(pwma,ain1,ain2) 00064 TB6612 motor_b(D10,D8,D9); //モータB制御用(pwmb,bin1,bin2) 00065 AQM0802 lcd(I2C_SDA,I2C_SCL); //液晶制御用 00066 00067 //使用変数の定義 00068 int Sw_Ptn; 00069 int Old_Sw_Ptn; 00070 double S1_Data,S2_Data,S3_Data,S4_Data,S5_Data,S6_Data,S7_Data,S8_Data; 00071 double All_Sensor_Data; //ラインセンサ総データ量 00072 double Sensor_Diff[2]={0,0}; //ラインセンサ偏差 00073 double Sensor_P =0.0f; //ラインセンサP(比例成分)制御量 00074 double Sensor_D =0.0f; //ラインセンサD(微分成分)制御量 00075 double Sensor_PD=0.0f; //ラインセンサP,D成分の合計 00076 char Gray_Str[5]; //LCD閾値表示用文字列 00077 float Gray=DEFAULT_GRAY; 00078 long int Enc_Count_A=0,Enc_Count_B=0; //エンコーダパルス数を格納 00079 long int Enc_A_Rotate=0,Enc_B_Rotate=0; 00080 00081 long int Distance_A=0,Distance_B=0; //タイヤ移動距離を格納[mm] 00082 long int Marker_Run_Distance=0; 00083 long int Speed_A=0, Speed_B=0; //現在速度 00084 long int Machine_Speed=DEFAULT_SPEED; 00085 char Speed_Str[5]; //LCD速度表示用文字列 00086 long int Target_Speed_A=0,Target_Speed_B=0; //目標速度 00087 long int Motor_A_Diff[2]={0,0}; //過去の速度偏差と現在の速度偏差を格納 00088 long int Motor_B_Diff[2]={0,0}; // 00089 float Motor_A_P,Motor_B_P; //モータ速度制御P成分 00090 float Motor_A_D,Motor_B_D; //モータ速度制御D成分 00091 float Motor_A_PD,Motor_B_PD; //モータ速度制御PD合成 00092 float Motor_A_Pwm,Motor_B_Pwm; //モータへの出力 00093 unsigned char Sensor_Digital =0x00; 00094 unsigned char Old_Sensor_Digital=0x00; 00095 int Sensor_Cnt=0; 00096 unsigned char Machine_Status =STOP; //機体状態 00097 unsigned char Old_Machine_Status=0x00; //過去の機体状態 00098 int Marker_Pass_Flag = 0; 00099 int Old_Marker_Pass_Flag=0; 00100 int Corner_Flag=0; 00101 int SG_Flag=0; 00102 int SG_Cnt=0; 00103 int Cross_Flag=0; 00104 void sensor_analog_read(){ 00105 S1_Data=s1.read(); 00106 S2_Data=s2.read(); 00107 S3_Data=s3.read(); 00108 S4_Data=s4.read(); 00109 S5_Data=s5.read(); 00110 S6_Data=s6.read(); 00111 S7_Data=s7.read(); 00112 S8_Data=s8.read(); 00113 } 00114 00115 00116 void sensor_digital_read(){//8つのフォトリフレクタの入力を8ビットのデジタルパターンに変換 00117 Sensor_Cnt=0; 00118 Old_Sensor_Digital=Sensor_Digital; 00119 if(S1_Data>Gray){ 00120 Sensor_Digital |= 0x80; //7ビット目のみセット (1にする。) 00121 }else Sensor_Digital &= 0x7F; //7ビット目のみマスク(0にする。) 00122 if(S2_Data>Gray){ 00123 Sensor_Digital |= 0x40; //6ビット目のみセット (1にする。) 00124 Sensor_Cnt++; 00125 }else Sensor_Digital &= 0xBF; //6ビット目のみマスク(0にする。) 00126 if(S3_Data>Gray){ 00127 Sensor_Digital |= 0x20; //5ビット目のみセット (1にする。) 00128 Sensor_Cnt++; 00129 }else Sensor_Digital &= 0xDF; //5ビット目のみマスク(0にする。) 00130 if(S4_Data>Gray){ 00131 Sensor_Digital |= 0x10; //4ビット目のみセット (1にする。) 00132 Sensor_Cnt++; 00133 }else Sensor_Digital &= 0xEF; //4ビット目のみマスク(0にする。) 00134 if(S5_Data>Gray){ 00135 Sensor_Digital |= 0x08; //3ビット目のみセット (1にする。) 00136 Sensor_Cnt++; 00137 }else Sensor_Digital &= 0xF7; //3ビット目のみマスク(0にする。) 00138 if(S6_Data>Gray){ 00139 Sensor_Digital |= 0x04; //2ビット目のみセット (1にする。) 00140 Sensor_Cnt++; 00141 }else Sensor_Digital &= 0xFB; //2ビット目のみマスク(0にする。) 00142 if(S7_Data>Gray){ 00143 Sensor_Digital |= 0x02; //1ビット目のみセット (1にする。) 00144 Sensor_Cnt++; 00145 }else Sensor_Digital &= 0xFD; //1ビット目のみマスク(0にする。) 00146 if(S8_Data>Gray){ 00147 Sensor_Digital |= 0x01; //0ビット目のみセット (1にする。) 00148 }else Sensor_Digital &= 0xFE; //0ビット目のみマスク(0にする。) 00149 } 00150 00151 void Machine_Status_Set(){ 00152 Old_Machine_Status=Machine_Status; 00153 00154 //機体がライン中央に位置するとき 00155 if(Sensor_Digital&RUN_COURSE_CENTER )Machine_Status|=RUN_COURSE_CENTER; 00156 else Machine_Status &= 0xE7;//ライン中央情報のマスク 00157 if((Sensor_Digital==0x00)&&(Old_Sensor_Digital==0x40)){//左センサコースアウト時 00158 Machine_Status|=RUN_COURSE_LOUT;//左コースアウト状態のビットをセット 00159 }else if((Machine_Status&RUN_COURSE_LOUT)&&(Sensor_Digital&RUN_COURSE_CENTER)){ 00160 //左コースアウト状態かつ機体がライン中央に復帰したとき 00161 Machine_Status &= 0xDF;//左コースアウト情報のみマスク 00162 } 00163 if((Sensor_Digital==0x00)&&(Old_Sensor_Digital==0x02)){//右センサコースアウト時 00164 Machine_Status|=RUN_COURSE_ROUT;//右コースアウト状態のビットをセット 00165 }else if((Machine_Status&RUN_COURSE_ROUT)&&(Sensor_Digital&RUN_COURSE_CENTER)){ 00166 //右コースアウト状態かつ機体がライン中央に復帰したとき 00167 Machine_Status &= 0xFB;//右コースアウト情報のみマスク 00168 } 00169 } 00170 00171 00172 //タイマ割り込み1[ms]周期 00173 void timer_interrupt(){ 00174 00175 //ラインセンサ情報取得 00176 sensor_analog_read(); 00177 sensor_digital_read(); 00178 00179 //機体状態の取得 00180 Machine_Status_Set(); 00181 00182 //交差点の認識 00183 if(Sensor_Cnt>=CROSS_JUDGE )Cross_Flag=1;//ラインセンサ4つ以上検知状態の時は交差点を示す。 00184 00185 //各種マーカの検知 00186 Old_Marker_Pass_Flag=Marker_Pass_Flag;//過去のフラグを退避 00187 if(Sensor_Digital&0x81){ //マーカセンサ検知時 00188 Marker_Pass_Flag=1; //マーカ通過中フラグをON 00189 if(Sensor_Digital&0x80)Corner_Flag=1; //コーナセンサの検知 00190 if(Sensor_Digital&0x01)SG_Flag=1; //スタートゴールセンサの検知 00191 if((Corner_Flag==1)&&(SG_Flag==1));//交差点通過中。何もしない 00192 }else Marker_Pass_Flag=0;//マーカ通過終了 00193 00194 //マーカ通過後、マーカ種類判別 00195 if((Old_Marker_Pass_Flag==1)&&(Marker_Pass_Flag==0)){//マーカ通過後 00196 if(Marker_Run_Distance>MARKER_WIDTH){//マーカ幅がもっともらしいとき 00197 if(Cross_Flag==1);//交差点の時は何もしない 00198 else if((SG_Flag==1)&&(SG_Cnt==0)){//ゴールスタートマーカの時⇒1回目 00199 SG_Cnt=1; 00200 }else if((SG_Flag==1)&&(SG_Cnt==1)){//ゴールスタートマーカの時⇒2回目 00201 Machine_Status|=STOP; //機体停止状態へ 00202 SG_Cnt=0; 00203 }else if(Corner_Flag==1){//コーナマーカの時 00204 //地区大会では何もしない 00205 } 00206 }else{//マーカではなく、誤検知だった場合。 00207 //何もしない 00208 } 00209 Corner_Flag=0; 00210 SG_Flag=0; 00211 Cross_Flag=0; 00212 Marker_Run_Distance=0;//マーカ通過距離情報リセット 00213 } 00214 00215 //センサ取得値の重ね合わせ(端のセンサほどモータ制御量を大きくする) 00216 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); 00217 Sensor_Diff[1]=Sensor_Diff[0];//過去のラインセンサ偏差を退避 00218 Sensor_Diff[0]=All_Sensor_Data; 00219 Sensor_P=All_Sensor_Data*S_KP; //ラインセンサ比例成分の演算 00220 Sensor_D=(Sensor_Diff[0]-Sensor_Diff[1])*S_KD; //ラインセンサ微分成分の演算 00221 Sensor_PD=Sensor_P+Sensor_D; 00222 00223 00224 ////モータ現在速度の取得 00225 Enc_Count_A=encoder_a.Get(); //エンコーダパルス数を取得 00226 Enc_Count_B=-encoder_b.Get(); 00227 Distance_A=(Enc_Count_A*PULSE_TO_UM); //移動距離をmm単位で格納 00228 Distance_B=(Enc_Count_B*PULSE_TO_UM); 00229 Speed_A=(Distance_A*1000)/INTERRUPT_TIME;//走行速度演算[mm/s] 00230 Speed_B=(Distance_B*1000)/INTERRUPT_TIME; 00231 00232 if(Machine_Status&STOP){//機体停止状態の時 00233 Enc_A_Rotate+=Enc_Count_A;//閾値用に左エンコーダ値の蓄積 00234 if(Enc_A_Rotate<-6400)Enc_A_Rotate=-6400; 00235 if(Enc_A_Rotate>6400)Enc_A_Rotate=6400; 00236 Enc_B_Rotate+=Enc_Count_B;//速度用に右エンコーダ値の蓄積 00237 if(Enc_B_Rotate<-6400)Enc_B_Rotate=-6400; 00238 if(Enc_B_Rotate>6400)Enc_B_Rotate=6400; 00239 } 00240 if(Marker_Pass_Flag==1){//マーカ通過中は通過距離情報を蓄積する。 00241 Marker_Run_Distance+=(Distance_A+Distance_B)/2; 00242 } 00243 //エンコーダ関連情報のリセット 00244 Distance_A=0; 00245 Distance_B=0; 00246 encoder_a.Set(0); 00247 encoder_b.Set(0); 00248 00249 /////各モータの目標速度の設定 00250 Target_Speed_A=Machine_Speed; 00251 Target_Speed_B=Machine_Speed; 00252 00253 /////モータの速度制御 00254 //過去の速度偏差を退避 00255 Motor_A_Diff[1]=Motor_A_Diff[0]; 00256 Motor_B_Diff[1]=Motor_B_Diff[0]; 00257 //現在の速度偏差を取得。 00258 Motor_A_Diff[0]=(Target_Speed_A-Speed_A); 00259 Motor_B_Diff[0]=(Target_Speed_B-Speed_B); 00260 //P成分演算 00261 Motor_A_P=Motor_A_Diff[0]*M_KP; 00262 Motor_B_P=Motor_B_Diff[0]*M_KP; 00263 //D成分演算 00264 Motor_A_D=(Motor_A_Diff[0]-Motor_A_Diff[1])*M_KD; 00265 Motor_B_D=(Motor_B_Diff[0]-Motor_B_Diff[1])*M_KD; 00266 //モータ速度制御のPD合成 00267 Motor_A_PD=Motor_A_P+Motor_A_D; 00268 Motor_B_PD=Motor_B_P+Motor_B_D; 00269 //最終的なモータ制御量の合成 00270 Motor_A_Pwm=Motor_A_PD+Sensor_PD; 00271 Motor_B_Pwm=Motor_B_PD-Sensor_PD; 00272 00273 //モータ制御量の上限下限設定 00274 if(Motor_A_Pwm>0.95f)Motor_A_Pwm=0.95f; 00275 else if(Motor_A_Pwm<-0.95)Motor_A_Pwm=-0.95f; 00276 if(Motor_B_Pwm>0.95f)Motor_B_Pwm=0.95f; 00277 else if(Motor_B_Pwm<-0.95f)Motor_B_Pwm=-0.95f; 00278 00279 //モータへの出力 00280 if(!(Machine_Status&STOP)){//マシンが停止状態でなければ 00281 if(Machine_Status&RUN_COURSE_LOUT){ //左端センサ振り切れた時 00282 motor_a=-(-TURN_POWER); //左旋回 00283 motor_b=-(TURN_POWER); 00284 }else if(Machine_Status&RUN_COURSE_ROUT){ //右端センサ振り切れた時 00285 motor_a=-(TURN_POWER); //右旋回 00286 motor_b=-(-TURN_POWER); 00287 }else if(Cross_Flag==1){//交差点通過中 00288 motor_a=-0.3;//交差点なので控えめの速度で直進 00289 motor_b=-0.3; 00290 } 00291 else{ 00292 motor_a=-Motor_A_Pwm; 00293 motor_b=-Motor_B_Pwm; 00294 } 00295 }else{//停止状態の時はモータへの出力は無効 00296 motor_a=0; 00297 motor_b=0; 00298 } 00299 } 00300 00301 00302 00303 int main() { 00304 timer.attach_us(&timer_interrupt,INTERRUPT_TIME);//タイマ割り込みスタート 00305 lcd.cls();//表示クリア 00306 lcd.locate(0,0); 00307 lcd.print("STOP"); 00308 lcd.locate(0,1); 00309 sprintf(Speed_Str,"%04d",Machine_Speed); 00310 lcd.print(Speed_Str); 00311 while(1){ 00312 Old_Sw_Ptn=Sw_Ptn; //過去のスイッチ入力情報を退避 00313 Sw_Ptn=push_sw; //現在のスイッチ入力情報の取得 00314 if((!(Old_Machine_Status&STOP))&&(Machine_Status&STOP)){//走行終了時 00315 lcd.locate(0,0); 00316 lcd.print(" "); 00317 lcd.locate(0,0); 00318 lcd.print("STOP"); 00319 } 00320 if(Machine_Status&STOP){//機体停止状態の時 00321 00322 Gray=DEFAULT_GRAY+((float)Enc_A_Rotate/16000);//センサ閾値調整 00323 sprintf(Gray_Str,"%3.2f",Gray);//速度情報文字列変換 00324 lcd.locate(0,0); 00325 lcd.print(" "); 00326 lcd.locate(0,0); 00327 lcd.print(Gray_Str); 00328 Machine_Speed=DEFAULT_SPEED+(Enc_B_Rotate/16);//標準速度調整 00329 sprintf(Speed_Str,"%04d",Machine_Speed);//速度情報文字列変換 00330 lcd.locate(0,1); 00331 lcd.print(" "); 00332 lcd.locate(0,1); 00333 lcd.print(Speed_Str); 00334 } 00335 00336 00337 if((Old_Sw_Ptn==PULL)&&(Sw_Ptn==PUSH)){//スイッチが押された瞬間 00338 if(Machine_Status&STOP){//機体停止状態の時 00339 lcd.locate(0,0); 00340 lcd.print(" "); 00341 lcd.locate(0,0); 00342 lcd.print("GO!!"); 00343 wait(2); 00344 Machine_Status&=0x7F;//ストップ状態解除 00345 }else{//機体走行中であったとき 00346 //各種フラグのクリア 00347 Corner_Flag=0; 00348 SG_Flag=0; 00349 Cross_Flag=0; 00350 Marker_Run_Distance=0;//マーカ通過距離情報リセット 00351 00352 Machine_Status |= STOP;//機体停止状態にする。 00353 lcd.locate(0,0); 00354 lcd.print(" "); 00355 lcd.locate(0,0); 00356 lcd.print("STOP"); 00357 } 00358 } 00359 } 00360 }
Generated on Fri Aug 5 2022 22:36:15 by 1.7.2