島根短大 ライントレーサ制作チーム
β版コースアウト時の旋回動作を導入。
Dependencies: mbed CRotaryEncoder TB6612FNG
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 ////ライントレースサンプルver3 00002 ///モータ速度制御プログラムを追加。 00003 ///ラインセンサPD制御を追加。 00004 ///M_KP,M_KDは別途straight_speed_controlのプログラムで検証した値をセットする。 00005 ///フォトリフレクタのゲイン、ラインセンサの各種成分の調整を行い、 00006 ///ラインをきれいにトレースできるよう挑戦する。 00007 00008 #include "mbed.h" 00009 #include "CRotaryEncoder.h" 00010 #include "TB6612.h" 00011 00012 00013 //☆★☆★各種パラメータ調整箇所☆★☆★☆★ 00014 #define DEFAULT_SPEED 900 //機体の直進速度1000[mm/s] 00015 #define TURN_POWER 0.5 //コースアウト時の旋回力 00016 #define PULSE_TO_UM 30 //エンコーダ1パルス当たりのタイヤ移動距離[um] 00017 #define INTERRUPT_TIME 1000 //割りこみ周期[us] 00018 #define GRAY 0.2f //フォトリフレクタデジタル入力の閾値 00019 //モータ速度のゲイン関連(むやみに調整しない) 00020 #define M_KP 0.002f //P(比例)制御成分 00021 #define M_KD 0.001f //D(微分)制御成分 00022 00023 //フォトリフレクタのゲイン（外側に行くにつれ値を何倍させたいか調整する。） 00024 #define S_K1 1.0f //float演算させる値には必ずfを付ける 00025 #define S_K2 2.0f //2倍 00026 #define S_K3 4.0f //4倍 00027 00028 //ラインセンサ各種制御成分 00029 #define S_KP 0.5f //ラインセンサ比例成分。大きいほど曲がりやすい 00030 #define S_KD 0.3f //ラインセンサ微分成分。大きいほど急なラインずれに強くなる。 00031 00032 00033 //////////☆★☆★☆★☆★☆★////////////// 00034 00035 //機体状態の定義 00036 #define STOP 0x80 //機体停止状態 00037 #define RUN_START 0x40 //スタートマーカ通過 00038 #define RUN_COURSE_LOUT 0x20 //左コースアウト状態 00039 #define RUN_COURSE_CENTER 0x18 //ライン中央走行状態 00040 #define RUN_COURSE_ROUT 0x04 //右コースアウト状態 00041 #define SECOND_RUN 0x02 //機体停止状態 00042 #define TUARD_RUN 0x01 //機体設定モード 00043 00044 /////アナログ入力オブジェクト定義////////// 00045 AnalogIn s1(D3); 00046 AnalogIn s2(A6); 00047 AnalogIn s3(A5); 00048 AnalogIn s4(A4); 00049 AnalogIn s5(A3); 00050 AnalogIn s6(A2); 00051 AnalogIn s7(A1); 00052 AnalogIn s8(A0); 00053 /////////////////////////////////////// 00054 Serial PC(USBTX,USBRX); 00055 CRotaryEncoder encoder_a(D1,D0); //モータAのエンコーダ 00056 CRotaryEncoder encoder_b(D11,D12); //モータBのエンコーダ 00057 Ticker timer; //タイマ割込み用 00058 TB6612 motor_a(D2,D7,D6); //モータA制御用(pwma,ain1,ain2) 00059 TB6612 motor_b(D10,D8,D9); //モータB制御用(pwmb,bin1,bin2) 00060 00061 00062 //使用変数の定義 00063 float S1_Data,S2_Data,S3_Data,S4_Data,S5_Data,S6_Data,S7_Data,S8_Data; 00064 float All_Sensor_Data; //ラインセンサ総データ量 00065 float Sensor_Diff[2]={0,0}; //ラインセンサ偏差 00066 float Sensor_P=0.0f; //ラインセンサP(比例成分)制御量 00067 float Sensor_D=0.0f; //ラインセンサD(微分成分)制御量 00068 float Sensor_PD=0.0f; //ラインセンサP,D成分の合計 00069 long int Enc_Count_A=0,Enc_Count_B=0; //エンコーダパルス数を格納 00070 long int Distance_A=0,Distance_B=0; //タイヤ移動距離を格納[mm] 00071 long int Speed_A=0, Speed_B=0; //現在速度 00072 long int Target_Speed_A=0,Target_Speed_B=0; //目標速度 00073 long int Motor_A_Diff[2]={0,0}; //過去の偏差と現在の偏差を格納 00074 long int Motor_B_Diff[2]={0,0}; 00075 float Motor_A_P,Motor_B_P; //モータP制御成分 00076 float Motor_A_D,Motor_B_D; //モータD制御成分 00077 float Motor_A_Pwm,Motor_B_Pwm; //モータへの出力 00078 unsigned char Sensor_Digital =0x00; 00079 unsigned char Old_Sensor_Digital=0x00; 00080 unsigned char Machine_Status =0x00; //機体状態 00081 unsigned char Old_Machine_Status=0x00; //過去の機体状態 00082 00083 00084 void sensor_analog_read(){ 00085 S1_Data=s1.read(); 00086 S2_Data=s2.read(); 00087 S3_Data=s3.read(); 00088 S4_Data=s4.read(); 00089 S5_Data=s5.read(); 00090 S6_Data=s6.read(); 00091 S7_Data=s7.read(); 00092 S8_Data=s8.read(); 00093 } 00094 void sensor_digital_read(){//８つのフォトリフレクタの入力を8ビットのデジタルパターンに変換 00095 Old_Sensor_Digital=Sensor_Digital; 00096 if(S1_Data>GRAY)Sensor_Digital |= 0x80; //7ビット目のみセット (1にする。) 00097 else Sensor_Digital &= 0x7F; //7ビット目のみマスク（0にする。） 00098 if(S2_Data>GRAY)Sensor_Digital |= 0x40; //6ビット目のみセット (1にする。) 00099 else Sensor_Digital &= 0xBF; //6ビット目のみマスク（0にする。） 00100 if(S3_Data>GRAY)Sensor_Digital |= 0x20; //5ビット目のみセット (1にする。) 00101 else Sensor_Digital &= 0xDF; //5ビット目のみマスク（0にする。） 00102 if(S4_Data>GRAY)Sensor_Digital |= 0x10; //4ビット目のみセット (1にする。) 00103 else Sensor_Digital &= 0xEF; //4ビット目のみマスク（0にする。） 00104 if(S5_Data>GRAY)Sensor_Digital |= 0x08; //3ビット目のみセット (1にする。) 00105 else Sensor_Digital &= 0xF7; //3ビット目のみマスク（0にする。） 00106 if(S6_Data>GRAY)Sensor_Digital |= 0x04; //2ビット目のみセット (1にする。) 00107 else Sensor_Digital &= 0xFB; //2ビット目のみマスク（0にする。） 00108 if(S7_Data>GRAY)Sensor_Digital |= 0x02; //1ビット目のみセット (1にする。) 00109 else Sensor_Digital &= 0xFD; //1ビット目のみマスク（0にする。） 00110 if(S8_Data>GRAY)Sensor_Digital |= 0x01; //0ビット目のみセット (1にする。) 00111 else Sensor_Digital &= 0xFE; //0ビット目のみマスク（0にする。） 00112 } 00113 00114 void Machine_Status_Set(){ 00115 Old_Machine_Status=Machine_Status; 00116 //機体がライン中央に位置するとき 00117 if(Sensor_Digital&RUN_COURSE_CENTER )Machine_Status|=RUN_COURSE_CENTER; 00118 else Machine_Status &= 0xE7;//ライン中央情報のマスク 00119 if((Sensor_Digital==0x00)&&(Old_Sensor_Digital==0x40)){//左センサコースアウト時 00120 Machine_Status|=RUN_COURSE_LOUT;//左コースアウト状態のビットをセット 00121 }else if((Machine_Status&RUN_COURSE_LOUT)&&(Sensor_Digital&RUN_COURSE_CENTER)){ 00122 //左コースアウト状態かつ機体がライン中央に復帰したとき 00123 Machine_Status &= 0xDF;//左コースアウト情報のみマスク 00124 } 00125 if((Sensor_Digital==0x00)&&(Old_Sensor_Digital==0x02)){//右センサコースアウト時 00126 Machine_Status|=RUN_COURSE_ROUT;//右コースアウト状態のビットをセット 00127 }else if((Machine_Status&RUN_COURSE_ROUT)&&(Sensor_Digital&RUN_COURSE_CENTER)){ 00128 //右コースアウト状態かつ機体がライン中央に復帰したとき 00129 Machine_Status &= 0xFB;//右コースアウト情報のみマスク 00130 } 00131 } 00132 00133 void timer_interrupt(){ 00134 //ラインセンサ情報取得 00135 sensor_analog_read(); 00136 sensor_digital_read(); 00137 Machine_Status_Set(); 00138 00139 //センサ取得値の重ね合わせ（端のセンサほどモータ制御量を大きくする） 00140 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); 00141 Sensor_Diff[1]=Sensor_Diff[0];//過去のラインセンサ偏差を退避 00142 Sensor_Diff[0]=All_Sensor_Data; 00143 Sensor_P=All_Sensor_Data*S_KP; //ラインセンサ比例成分の演算 00144 Sensor_D=(Sensor_Diff[0]-Sensor_Diff[1])*S_KD; //ラインセンサ微分成分の演算 00145 Sensor_PD=Sensor_P+Sensor_D; 00146 00147 00148 ////モータ現在速度の取得 00149 Enc_Count_A=encoder_a.Get(); //エンコーダパルス数を取得 00150 Enc_Count_B=-encoder_b.Get(); 00151 Distance_A=(Enc_Count_A*PULSE_TO_UM); //移動距離をmm単位で格納 00152 Distance_B=(Enc_Count_B*PULSE_TO_UM); 00153 Speed_A=(Distance_A*1000)/INTERRUPT_TIME;//走行速度演算[mm/s] 00154 Speed_B=(Distance_B*1000)/INTERRUPT_TIME; 00155 Distance_A=0; 00156 Distance_B=0; 00157 encoder_a.Set(0); 00158 encoder_b.Set(0); 00159 00160 /////各モータの目標速度の設定 00161 Target_Speed_A=DEFAULT_SPEED; 00162 Target_Speed_B=DEFAULT_SPEED; 00163 00164 /////モータの速度制御 00165 //過去の速度偏差を退避 00166 Motor_A_Diff[1]=Motor_A_Diff[0]; 00167 Motor_B_Diff[1]=Motor_B_Diff[0]; 00168 //現在の速度偏差を取得。 00169 Motor_A_Diff[0]=(Target_Speed_A-Speed_A); 00170 Motor_B_Diff[0]=(Target_Speed_B-Speed_B); 00171 //P成分演算 00172 Motor_A_P=Motor_A_Diff[0]*M_KP; 00173 Motor_B_P=Motor_B_Diff[0]*M_KP; 00174 //D成分演算 00175 Motor_A_D=(Motor_A_Diff[0]-Motor_A_Diff[1])*M_KD; 00176 Motor_B_D=(Motor_B_Diff[0]-Motor_B_Diff[1])*M_KD; 00177 00178 Motor_A_Pwm=Motor_A_P+Motor_A_D+Sensor_PD; 00179 Motor_B_Pwm=Motor_B_P+Motor_B_D-Sensor_PD; 00180 if(Motor_A_Pwm>0.95f)Motor_A_Pwm=0.95f; 00181 else if(Motor_A_Pwm<-0.95)Motor_A_Pwm=-0.95f; 00182 if(Motor_B_Pwm>0.95f)Motor_B_Pwm=0.95f; 00183 else if(Motor_B_Pwm<-0.95f)Motor_B_Pwm=-0.95f; 00184 00185 //モータへの出力 00186 00187 if(!(Machine_Status&STOP)){//マシンが停止状態でなければ 00188 if(Machine_Status&RUN_COURSE_LOUT){ 00189 motor_a=-(-TURN_POWER); 00190 motor_b=-(TURN_POWER); 00191 }else if(Machine_Status&RUN_COURSE_ROUT){ 00192 motor_a=-(TURN_POWER); 00193 motor_b=-(-TURN_POWER); 00194 }else{ 00195 motor_a=-Motor_A_Pwm; 00196 motor_b=-Motor_B_Pwm; 00197 } 00198 }else{//停止状態の時はモータへの出力は無効 00199 motor_a=0; 00200 motor_b=0; 00201 } 00202 } 00203 00204 int main() { 00205 timer.attach_us(&timer_interrupt,INTERRUPT_TIME);//タイマ割り込みスタート 00206 while(1){ 00207 wait(1); 00208 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"\ 00209 ,Sensor_Digital,Old_Sensor_Digital\ 00210 ,Machine_Status,Old_Machine_Status);//表示 00211 } 00212 }
Generated on Sat Jul 16 2022 02:00:17 by
1.7.2