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