lab6
Dependencies: mbed
Fork of Robotics_LAB_UART by
main.cpp
00001 #include "mbed.h" 00002 00003 #define Ts 0.01f //period of timer1 (s) 00004 #define Kp 0.005f 00005 #define Ki 0.02f 00006 00007 Ticker timer1; 00008 Ticker timer2; 00009 Serial bt(D10, D2); // TXpin, RXpin 00010 00011 // servo motor 00012 PwmOut servo_cmd(A0); 00013 // DC motor 00014 PwmOut pwm1(D7); 00015 PwmOut pwm1n(D11); 00016 PwmOut pwm2(D8); 00017 PwmOut pwm2n(A3); 00018 00019 // Motor1 sensor 00020 InterruptIn HallA(A1); 00021 InterruptIn HallB(A2); 00022 // Motor2 sensor 00023 InterruptIn HallA_2(D13); 00024 InterruptIn HallB_2(D12); 00025 00026 // servo motor 00027 float servo_duty = 0.025; // 0.069 +(0.088/180)*angle, -90<angle<90 00028 // 90度->duty=0.025; 0度->duty=0.069; -90度->duty=0.113 00029 int angle = 0; 00030 int counter; 00031 00032 //RX 00033 int readcount = 0; 00034 int RX_flag1 = 0; 00035 int RX_flag2 = 0; 00036 char getData[6] = {0,0,0,0,0,0}; 00037 short data_received[3] = {0,0,0}; 00038 short data_received_old[3] = {0,0,0}; 00039 00040 //函式宣告 00041 void init_TIMER(); 00042 void timer1_ITR(); 00043 void timer2_ITR(); 00044 void init_UART(); 00045 void RX_ITR(); 00046 void init_IO(); 00047 void init_CN(); 00048 void CN_ITR(); 00049 void init_PWM(); 00050 00051 00052 // Hall sensor 00053 int HallA_1_state = 0; 00054 int HallB_1_state = 0; 00055 int state_1 = 0; 00056 int state_1_old = 0; 00057 int HallA_2_state = 0; 00058 int HallB_2_state = 0; 00059 int state_2 = 0; 00060 int state_2_old = 0; 00061 00062 // DC motor rotation speed control 00063 int speed_count_1 = 0; 00064 float rotation_speed_1 = 0.0; 00065 float rotation_speed_ref_1 = 0; 00066 float pwm1_duty = 0.5; 00067 float PI_out_1 = 0.0; 00068 float err_1 = 0.0; 00069 float ierr_1 = 0.0; 00070 int speed_count_2 = 0; 00071 float rotation_speed_2 = 0.0; 00072 float rotation_speed_ref_2 = 0; 00073 float pwm2_duty = 0.5; 00074 float PI_out_2 = 0.0; 00075 float err_2 = 0.0; 00076 float ierr_2 = 0.0; 00077 00078 int main() 00079 { 00080 init_PWM(); 00081 init_CN(); 00082 init_TIMER(); 00083 init_UART(); 00084 while(1) { 00085 } 00086 } 00087 00088 void init_TIMER() 00089 { 00090 timer1.attach_us(&timer1_ITR, 10000.0); // the address of the function to be attached (timer1_ITR) and the interval (1000 micro-seconds) 00091 timer2.attach_us(&timer2_ITR, 10000.0); 00092 } 00093 00094 void init_UART() 00095 { 00096 bt.baud(115200); // baud rate設為115200 00097 bt.attach(&RX_ITR, Serial::RxIrq); // Attach a function(RX_ITR) to call whenever a serial interrupt is generated. 00098 } 00099 00100 void timer1_ITR() 00101 { 00102 // 避免收到錯誤資料,若超出設定範圍則用上次的資料 00103 if(data_received[0]>300 || data_received[0]<-300 || data_received[1]>300 || data_received[1]<-300 || data_received[2]>90 || data_received[0]<-90) { 00104 data_received[0] = data_received_old[0]; 00105 data_received[1] = data_received_old[1]; 00106 data_received[2] = data_received_old[2]; 00107 } else { 00108 data_received_old[0] = data_received[0]; 00109 data_received_old[1] = data_received[1]; 00110 data_received_old[2] = data_received[2]; 00111 } 00112 rotation_speed_ref_1 = data_received[0]; 00113 rotation_speed_ref_2 = (-1)*data_received[1]; 00114 00115 } 00116 00117 void RX_ITR() 00118 { 00119 while(bt.readable()) { 00120 static char uart_read; 00121 uart_read = bt.getc(); 00122 if(uart_read == 127 && RX_flag1 == 1) { 00123 RX_flag2 = 1; 00124 } else { 00125 RX_flag1 = 0; 00126 } 00127 00128 if(RX_flag2 == 1) { 00129 getData[readcount] = uart_read; 00130 readcount++; 00131 if(readcount >= 5) { 00132 readcount = 0; 00133 RX_flag2 = 0; 00134 ///code for decoding/// 00135 data_received[0] = (getData[2] << 8) | getData[1]; 00136 data_received[1] = (getData[4] << 8) | getData[3]; 00137 data_received[2] = getData[5]; 00138 /////////////////////// 00139 } 00140 } else if(uart_read == 254 && RX_flag1 == 0) { 00141 RX_flag1 = 1; 00142 } 00143 } 00144 } 00145 00146 void init_PWM() 00147 { 00148 servo_cmd.period_ms(20); 00149 servo_cmd.write(servo_duty); 00150 00151 pwm1.period_us(50); 00152 pwm1.write(0.5); 00153 TIM1->CCER |= 0x4; 00154 00155 pwm2.period_us(50); 00156 pwm2.write(0.5); 00157 TIM1->CCER |= 0x40; 00158 } 00159 void init_CN() 00160 { 00161 HallA.rise(&CN_ITR); 00162 HallA.fall(&CN_ITR); 00163 HallB.rise(&CN_ITR); 00164 HallB.fall(&CN_ITR); 00165 00166 HallA_2.rise(&CN_ITR); 00167 HallA_2.fall(&CN_ITR); 00168 HallB_2.rise(&CN_ITR); 00169 HallB_2.fall(&CN_ITR); 00170 } 00171 00172 void CN_ITR() 00173 { 00174 // motor1 00175 HallA_1_state = HallA.read(); 00176 HallB_1_state = HallB.read(); 00177 00178 ///code for state determination/// 00179 00180 state_1 = 10*HallA_1_state + HallB_1_state; //state = AB (ex:A=1,B=0, state_1 = 10) 00181 00182 if(state_1_old != state_1) 00183 { 00184 if((state_1_old/10) == (state_1_old%10)) 00185 { 00186 if((state_1%10) != (state_1_old%10)) 00187 { 00188 speed_count_1++; 00189 } 00190 else if((state_1/10) != (state_1_old/10)) 00191 { 00192 speed_count_1--; 00193 } 00194 } 00195 else if((state_1_old/10) != (state_1_old%10)) 00196 { 00197 if((state_1%10) != (state_1_old%10)) 00198 { 00199 speed_count_1--; 00200 } 00201 else if((state_1/10) != (state_1_old/10)) 00202 { 00203 speed_count_1++; 00204 } 00205 } 00206 00207 state_1_old = state_1; 00208 } 00209 00210 00211 ////////////////////////////////// 00212 00213 //forward : speed_count_1 + 1 00214 //backward : speed_count_1 - 1 00215 00216 00217 // motor2 00218 HallA_2_state = HallA_2.read(); 00219 HallB_2_state = HallB_2.read(); 00220 00221 ///code for state determination/// 00222 00223 state_2 = 10*HallA_2_state + HallB_2_state; //state = AB (ex:A=1,B=0, state_1 = 10) 00224 00225 if(state_2_old != state_2) 00226 { 00227 if((state_2_old/10) == (state_2_old%10)) 00228 { 00229 if((state_2%10) != (state_2_old%10)) 00230 { 00231 speed_count_2++; 00232 } 00233 else if((state_2/10) != (state_2_old/10)) 00234 { 00235 speed_count_2--; 00236 } 00237 } 00238 else if((state_2_old/10) != (state_2_old%10)) 00239 { 00240 if((state_2%10) != (state_2_old%10)) 00241 { 00242 speed_count_2--; 00243 } 00244 else if((state_2/10) != (state_2_old/10)) 00245 { 00246 speed_count_2++; 00247 } 00248 } 00249 00250 state_2_old = state_2; 00251 } 00252 00253 00254 00255 ////////////////////////////////// 00256 00257 //forward : speed_count_2 + 1 00258 //backward : speed_count_2 - 1 00259 } 00260 00261 void timer2_ITR() 00262 { 00263 // servo motor 00264 ///code for sevo motor/// 00265 00266 counter = counter + 1; 00267 00268 if(counter == 100) 00269 { 00270 servo_duty = 0.069; 00271 } 00272 if(counter == 200) 00273 { 00274 servo_duty = 0.0763; 00275 } 00276 if(counter == 300) 00277 { 00278 servo_duty = 0.0837; 00279 } 00280 if(counter == 400) 00281 { 00282 servo_duty = 0.091; 00283 } 00284 if(counter == 500) 00285 { 00286 servo_duty = 0.0983; 00287 } 00288 if(counter == 600) 00289 { 00290 servo_duty = 0.106; 00291 } 00292 00293 if(counter == 700) 00294 { 00295 servo_duty = 0.113; 00296 } 00297 if(counter > 700) 00298 { 00299 counter=0; 00300 } 00301 00302 servo_cmd.write(servo_duty); 00303 00304 00305 ///////////////////////// 00306 00307 if(servo_duty >= 0.113f)servo_duty = 0.113; 00308 else if(servo_duty <= 0.025f)servo_duty = 0.025; 00309 servo_cmd.write(servo_duty); 00310 00311 // motor1 00312 rotation_speed_1 = (float)speed_count_1 * 100.0f / 12.0f * 60.0f / 29.0f ; //unit: rpm 00313 speed_count_1 = 0; 00314 00315 ///PI controller for motor1/// 00316 00317 err_1 = rotation_speed_ref_1 - rotation_speed_1; 00318 ierr_1 = ierr_1 + err_1*Ts; 00319 PI_out_1 = Kp*err_1 + Ki*ierr_1; 00320 00321 ////////////////////////////// 00322 00323 if(PI_out_1 >= 0.5f)PI_out_1 = 0.5; 00324 else if(PI_out_1 <= -0.5f)PI_out_1 = -0.5; 00325 pwm1_duty = PI_out_1 + 0.5f; 00326 pwm1.write(PI_out_1 + 0.5f); 00327 TIM1->CCER |= 0x4; 00328 00329 //motor2 00330 rotation_speed_2 = (float)speed_count_2 * 100.0f / 12.0f * 60.0f / 29.0f; //unit: rpm 00331 speed_count_2 = 0; 00332 00333 ///PI controller for motor2/// 00334 00335 err_2 = rotation_speed_ref_2 - rotation_speed_2; 00336 ierr_2 = ierr_2 + err_2*Ts; 00337 PI_out_2 = Kp*err_2 + Ki*ierr_2; 00338 00339 ////////////////////////////// 00340 00341 if(PI_out_2 >= 0.5f)PI_out_2 = 0.5; 00342 else if(PI_out_2 <= -0.5f)PI_out_2 = -0.5; 00343 pwm2_duty = PI_out_2 + 0.5f; 00344 pwm2.write(PI_out_2 + 0.5f); 00345 TIM1->CCER |= 0x40; 00346 }
Generated on Mon Jul 18 2022 23:07:32 by
1.7.2
