final
Dependencies: mbed
Fork of Robotics_LAB_UART 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.025; // 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 float duty; 00076 00077 float err_1_old=0; 00078 float err_2_old=0; 00079 bool servo=1; 00080 00081 00082 Serial pc(USBTX,USBRX); 00083 ///////////////////////////////////////////////////////////// 00084 ///////////////////////////////////////////////////////////// 00085 ///////////////////////////////////////////////////////////// 00086 int main() 00087 { 00088 pc.baud(9600); 00089 init_TIMER(); 00090 init_UART(); 00091 init_PWM(); 00092 init_CN(); 00093 while(1) { 00094 } 00095 } 00096 00097 void init_TIMER() 00098 { 00099 timer1.attach_us(&timer1_ITR, 10000.0); // the address of the function to be attached (timer1_ITR) and the interval (1000 micro-seconds) 00100 } 00101 00102 void init_UART() 00103 { 00104 bt.baud(115200); // baud rate設為115200 00105 bt.attach(&RX_ITR, Serial::RxIrq); // Attach a function(RX_ITR) to call whenever a serial interrupt is generated. 00106 } 00107 00108 void timer1_ITR() 00109 { 00110 // 避免收到錯誤資料,若超出設定範圍則用上次的資料 00111 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) { 00112 data_received[0] = data_received_old[0]; 00113 data_received[1] = data_received_old[1]; 00114 data_received[2] = data_received_old[2]; 00115 } else { 00116 data_received_old[0] = data_received[0]; 00117 data_received_old[1] = data_received[1]; 00118 data_received_old[2] = data_received[2]; 00119 } 00120 00121 /* 00122 servo=1; 00123 while(servo==1){ 00124 if(i==0 || i==100 || i==200 || i==300 || i==400 || i==500 || i==600){ 00125 00126 duty=0.113f-0.0000733f*(float)i; 00127 00128 } 00129 //pc.printf("duty= %f ,\n",duty); 00130 //servo_cmd.period_ms(20); 00131 servo_cmd.write( data_received_old[2]); 00132 servo=0; 00133 i=i+1; 00134 } 00135 if(i==601){ 00136 i=0; 00137 } 00138 00139 00140 */ 00141 ///////////////////////// 00142 00143 if(servo_duty >= 0.113f)servo_duty = 0.113; 00144 else if(servo_duty <= 0.025f)servo_duty = 0.025; 00145 servo_cmd.write(servo_duty); 00146 00147 // motor1 00148 rotation_speed_1 = (float)speed_count_1 * 100.0f / 12.0f * 60.0f / 29.0f; //unit: rpm 00149 speed_count_1 = 0; 00150 00151 ///PI controller for motor1/// 00152 00153 err_1=(data_received_old[0]-rotation_speed_1)*p_gain; 00154 ierr_1=(err_1_old+err_1)*i_gain; 00155 PI_out_1=err_1+ierr_1; 00156 err_1_old=err_1; 00157 ////////////////////////////// 00158 00159 if(PI_out_1 >= 0.5f)PI_out_1 = 0.5; 00160 else if(PI_out_1 <= -0.5f)PI_out_1 = -0.5; 00161 pwm1_duty = PI_out_1 + 0.5f; 00162 pwm1.write(pwm1_duty); 00163 TIM1->CCER |= 0x4; 00164 00165 //motor2 00166 rotation_speed_2 = (float)speed_count_2 * 100.0f / 12.0f * 60.0f / 29.0f; //unit: rpm 00167 speed_count_2 = 0; 00168 00169 ///PI controller for motor2/// 00170 00171 err_2=(data_received_old[1]-rotation_speed_2)*p_gain; 00172 ierr_2=(err_2_old+err_2)*i_gain; 00173 PI_out_2=err_2+ierr_2; 00174 err_2_old=err_2; 00175 ////////////////////////////// 00176 00177 if(PI_out_2 >= 0.5f)PI_out_2 = 0.5; 00178 else if(PI_out_2 <= -0.5f)PI_out_2 = -0.5; 00179 pwm2_duty = -PI_out_2 + 0.5f; 00180 pwm2.write(pwm2_duty); 00181 TIM1->CCER |= 0x40; 00182 00183 //pc.printf("SPEED= %f,/n",data_received_old[1]); 00184 00185 } 00186 00187 void RX_ITR() 00188 { 00189 while(bt.readable()) { 00190 static char uart_read; 00191 uart_read = bt.getc(); 00192 if(uart_read == 127 && RX_flag1 == 1) { 00193 RX_flag2 = 1; 00194 } else { 00195 RX_flag1 = 0; 00196 } 00197 00198 if(RX_flag2 == 1) { 00199 getData[readcount] = uart_read; 00200 readcount++; 00201 if(readcount >= 5) { 00202 readcount = 0; 00203 RX_flag2 = 0; 00204 ///code for decoding/// 00205 data_received[0] = (getData[2] << 8) | getData[1]; 00206 data_received[1] = (getData[4] << 8) | getData[3]; 00207 data_received[2] = getData[5]; 00208 /////////////////////// 00209 } 00210 } 00211 else if(uart_read == 254 && RX_flag1 == 0) { 00212 RX_flag1 = 1; 00213 } 00214 } 00215 } 00216 00217 /////////////////////////////////////////////////////////////////// 00218 /////////////////////////////////////////////////////////////////// 00219 /////////////////////////////////////////////////////////////////// 00220 void init_PWM() 00221 { 00222 servo_cmd.period_ms(20); 00223 servo_cmd.write(servo_duty); 00224 00225 pwm1.period_us(50); 00226 pwm1.write(0.5); 00227 TIM1->CCER |= 0x4; 00228 00229 pwm2.period_us(50); 00230 pwm2.write(0.5); 00231 TIM1->CCER |= 0x40; 00232 } 00233 void init_CN() 00234 { 00235 HallA.rise(&CN_ITR); 00236 HallA.fall(&CN_ITR); 00237 HallB.rise(&CN_ITR); 00238 HallB.fall(&CN_ITR); 00239 00240 HallA_2.rise(&CN_ITR); 00241 HallA_2.fall(&CN_ITR); 00242 HallB_2.rise(&CN_ITR); 00243 HallB_2.fall(&CN_ITR); 00244 } 00245 00246 void CN_ITR() 00247 { 00248 00249 // motor1 00250 HallA_1_state = HallA.read(); 00251 HallB_1_state = HallB.read(); 00252 //led1 != led1; 00253 00254 ///code for state determination/// 00255 ////////////////////////////////// 00256 ////////////////////////////////// 00257 //determine the state 00258 if((HallA_1_state == 0)&&(HallB_1_state == 0)) 00259 { 00260 state_1 = 1; 00261 } 00262 else if((HallA_1_state == 0)&&(HallB_1_state == 1)) 00263 { 00264 state_1 = 2; 00265 } 00266 else if((HallA_1_state == 1)&&(HallB_1_state == 1)) 00267 { 00268 state_1 = 3; 00269 } 00270 else if((HallA_1_state == 1)&&(HallB_1_state ==0)) 00271 { 00272 state_1 = 4; 00273 } 00274 00275 //forward or backward 00276 int direction_1 = 0; 00277 direction_1 = state_1 - state_1_old; 00278 if((direction_1 == -1) || (direction_1 == 3)) 00279 { 00280 //forward 00281 speed_count_1 = speed_count_1 - 1; 00282 } 00283 else if((direction_1 == 1) || (direction_1 == -3)) 00284 { 00285 //backward 00286 speed_count_1 = speed_count_1 + 1; 00287 } 00288 else 00289 { 00290 //prevent initializing error 00291 state_1_old = state_1; 00292 } 00293 00294 //change old state 00295 state_1_old = state_1; 00296 ////////////////////////////////// 00297 ////////////////////////////////// 00298 //forward : speed_count_1 + 1 00299 //backward : speed_count_1 - 1 00300 00301 // motor2 00302 HallA_2_state = HallA_2.read(); 00303 HallB_2_state = HallB_2.read(); 00304 00305 ///code for state determination/// 00306 ////////////////////////////////// 00307 ///////////////////////////////// 00308 //determine the state 00309 if((HallA_2_state == 0)&&(HallB_2_state == 0)) 00310 { 00311 state_2 = 1; 00312 } 00313 else if((HallA_2_state == 0)&&(HallB_2_state == 1)) 00314 { 00315 state_2 = 2; 00316 } 00317 else if((HallA_2_state == 1)&&(HallB_2_state == 1)) 00318 { 00319 state_2 = 3; 00320 } 00321 else if((HallA_2_state == 1)&&(HallB_2_state ==0)) 00322 { 00323 state_2 = 4; 00324 } 00325 00326 //forward or backward 00327 int direction_2 = 0; 00328 direction_2 = state_2 - state_2_old; 00329 if((direction_2 == 1) || (direction_2 == -3)) 00330 { 00331 //forward 00332 speed_count_2 = speed_count_2 - 1; 00333 } 00334 else if((direction_2 == -1) || (direction_2 == 3)) 00335 { 00336 //backward 00337 speed_count_2 = speed_count_2 + 1; 00338 } 00339 else 00340 { 00341 //prevent initializing error 00342 state_2_old = state_2; 00343 } 00344 00345 //change old state 00346 state_2_old = state_2; 00347 ////////////////////////////////// 00348 ////////////////////////////////// 00349 //forward : speed_count_2 + 1 00350 //backward : speed_count_2 - 1 00351 }
Generated on Fri Aug 26 2022 02:31:42 by
1.7.2
