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