welcome referance
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.015; 00074 float i_gain=0.003; 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 // motor1 00249 HallA_1_state = HallA.read(); 00250 HallB_1_state = HallB.read(); 00251 00252 ///code for state determination/// 00253 if(state_1==1 && state_1_old==1){ 00254 if(HallA_1_state==1 && HallB_1_state==0){ 00255 speed_count_1=speed_count_1 + 1; 00256 } 00257 else if(HallA_1_state==0 && HallB_1_state==1){ 00258 speed_count_1=speed_count_1 - 1; 00259 } 00260 } 00261 if(state_1==1 && state_1_old==0){ 00262 if(HallA_1_state==0 && HallB_1_state==0){ 00263 speed_count_1=speed_count_1 + 1; 00264 } 00265 else if(HallA_1_state==1 && HallB_1_state==1){ 00266 speed_count_1=speed_count_1 - 1; 00267 } 00268 } 00269 if(state_1==0 && state_1_old==0){ 00270 if(HallA_1_state==0 && HallB_1_state==1){ 00271 speed_count_1=speed_count_1 + 1; 00272 } 00273 else if(HallA_1_state==1 && HallB_1_state==0){ 00274 speed_count_1=speed_count_1 - 1; 00275 } 00276 } 00277 if(state_1==0 && state_1_old==1){ 00278 if(HallA_1_state==1 && HallB_1_state==1){ 00279 speed_count_1=speed_count_1 + 1; 00280 } 00281 else if(HallA_1_state==0 && HallB_1_state==0){ 00282 speed_count_1=speed_count_1 - 1; 00283 } 00284 } 00285 state_1=HallA_1_state; 00286 state_1_old=HallB_1_state; 00287 ////////////////////////////////// 00288 00289 //forward : speed_count_1 + 1 00290 //backward : speed_count_1 - 1 00291 00292 00293 // motor2 00294 HallA_2_state = HallA_2.read(); 00295 HallB_2_state = HallB_2.read(); 00296 00297 ///code for state determination/// 00298 if(state_2==1 && state_2_old==1){ 00299 if(HallA_2_state==1 && HallB_2_state==0){ 00300 speed_count_2=speed_count_2 + 1; 00301 } 00302 else if(HallA_2_state==0 && HallB_2_state==1){ 00303 speed_count_2=speed_count_2 - 1; 00304 } 00305 } 00306 if(state_2==1 && state_2_old==0){ 00307 if(HallA_2_state==0 && HallB_2_state==0){ 00308 speed_count_2=speed_count_2 + 1; 00309 } 00310 else if(HallA_2_state==1 && HallB_2_state==1){ 00311 speed_count_2=speed_count_2 - 1; 00312 } 00313 } 00314 if(state_2==0 && state_2_old==0){ 00315 if(HallA_2_state==0 && HallB_2_state==1){ 00316 speed_count_2=speed_count_2 + 1; 00317 } 00318 else if(HallA_2_state==1 && HallB_2_state==0){ 00319 speed_count_2=speed_count_2 - 1; 00320 } 00321 } 00322 if(state_2==0 && state_2_old==1){ 00323 if(HallA_2_state==1 && HallB_2_state==1){ 00324 speed_count_2=speed_count_2 + 1; 00325 } 00326 else if(HallA_2_state==0 && HallB_2_state==0){ 00327 speed_count_2=speed_count_2 - 1; 00328 } 00329 } 00330 state_2=HallA_2_state; 00331 state_2_old=HallB_2_state; 00332 00333 00334 ////////////////////////////////// 00335 00336 //forward : speed_count_2 + 1 00337 //backward : speed_count_2 - 1 00338 }
Generated on Sun Aug 14 2022 21:55:49 by
1.7.2
