YICHUN SAVE US
Dependencies: mbed
Fork of FINALFINALNORMAL by
main.cpp
- Committer:
- YutingHsiao
- Date:
- 2017-06-08
- Revision:
- 3:6422dc6e8509
- Parent:
- 2:ae0ba466c714
- Child:
- 4:f245d6416dba
File content as of revision 3:6422dc6e8509:
#include "mbed.h" Ticker timer1; Serial bt(D10, D2); // TXpin, RXpin //RX int readcount = 0; int RX_flag1 = 0; int RX_flag2 = 0; char getData[6] = {0,0,0,0,0,0}; short data_received[3] = {0,0,0}; short data_received_old[3] = {0,0,0}; //函式宣告 void init_TIMER(); void timer1_ITR(); void init_UART(); void RX_ITR(); ///////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////// // servo motor PwmOut servo_cmd(A0); // DC motor PwmOut pwm1(D7); PwmOut pwm1n(D11); PwmOut pwm2(D8); PwmOut pwm2n(A3); // Motor1 sensor InterruptIn HallA(A1); InterruptIn HallB(A2); // Motor2 sensor InterruptIn HallA_2(D13); InterruptIn HallB_2(D12); void init_CN(); void CN_ITR(); void init_PWM(); // servo motor float servo_duty = 0.025; // 0.069 +(0.088/180)*angle, -90<angle<90 // 90度->duty=0.025; 0度->duty=0.069; -90度->duty=0.113 int angle = 0; // Hall sensor int HallA_1_state = 0; int HallB_1_state = 0; int state_1 = 0; int state_1_old = 0; int HallA_2_state = 0; int HallB_2_state = 0; int state_2 = 0; int state_2_old = 0; int i=0; // DC motor rotation speed control int speed_count_1 = 0; float rotation_speed_1 = 0.0; float rotation_speed_ref_1 = 0; float pwm1_duty = 0.5; float PI_out_1 = 0.0; float err_1 = 0.0; float ierr_1 = 0.0; int speed_count_2 = 0; float rotation_speed_2 = 0.0; float rotation_speed_ref_2 = 0; float pwm2_duty = 0.5; float PI_out_2 = 0.0; float err_2 = 0.0; float ierr_2 = 0.0; float p_gain=0.002; float i_gain=0.05; float err_1_old=0; float err_2_old=0; Serial pc(USBTX,USBRX); ///////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////// int main() { pc.baud(9600); init_TIMER(); init_UART(); init_PWM(); init_CN(); while(1) { } } void init_TIMER() { timer1.attach_us(&timer1_ITR, 10000.0); // the address of the function to be attached (timer1_ITR) and the interval (1000 micro-seconds) } void init_UART() { bt.baud(115200); // baud rate設為115200 bt.attach(&RX_ITR, Serial::RxIrq); // Attach a function(RX_ITR) to call whenever a serial interrupt is generated. } void timer1_ITR() { // 避免收到錯誤資料,若超出設定範圍則用上次的資料 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) { data_received[0] = data_received_old[0]; data_received[1] = data_received_old[1]; data_received[2] = data_received_old[2]; } else { data_received_old[0] = data_received[0]; data_received_old[1] = data_received[1]; data_received_old[2] = data_received[2]; } if( data_received_old[2] == 1) { //open // servo_duty = xxx; } if( data_received_old[2] == 2) { //closed // servo_duty = xxx; } if(servo_duty >= 0.113f)servo_duty = 0.113; else if(servo_duty <= 0.025f)servo_duty = 0.025; servo_cmd.write(servo_duty); // motor1 rotation_speed_1 = (float)speed_count_1 * 100.0f / 12.0f * 60.0f / 29.0f; //unit: rpm speed_count_1 = 0; ///PI controller for motor1/// err_1=(data_received_old[0]-rotation_speed_1)*p_gain; ierr_1=(err_1_old+err_1)*i_gain; PI_out_1=err_1+ierr_1; err_1_old=err_1; ////////////////////////////// if(PI_out_1 >= 0.5f)PI_out_1 = 0.5; else if(PI_out_1 <= -0.5f)PI_out_1 = -0.5; pwm1_duty = PI_out_1 + 0.5f; pwm1.write(pwm1_duty); TIM1->CCER |= 0x4; //motor2 rotation_speed_2 = (float)speed_count_2 * 100.0f / 12.0f * 60.0f / 29.0f; //unit: rpm speed_count_2 = 0; ///PI controller for motor2/// err_2=(data_received_old[1]-rotation_speed_2)*p_gain; ierr_2=(err_2_old+err_2)*i_gain; PI_out_2=err_2+ierr_2; err_2_old=err_2; ////////////////////////////// if(PI_out_2 >= 0.5f)PI_out_2 = 0.5; else if(PI_out_2 <= -0.5f)PI_out_2 = -0.5; pwm2_duty = -PI_out_2 + 0.5f; pwm2.write(pwm2_duty); TIM1->CCER |= 0x40; //pc.printf("SPEED= %f,/n",data_received_old[1]); } void RX_ITR() { while(bt.readable()) { static char uart_read; uart_read = bt.getc(); if(uart_read == 127 && RX_flag1 == 1) { RX_flag2 = 1; } else { RX_flag1 = 0; } if(RX_flag2 == 1) { getData[readcount] = uart_read; readcount++; if(readcount >= 5) { readcount = 0; RX_flag2 = 0; ///code for decoding/// data_received[0] = (getData[2] << 8) | getData[1]; data_received[1] = (getData[4] << 8) | getData[3]; data_received[2] = getData[5]; /////////////////////// } } else if(uart_read == 254 && RX_flag1 == 0) { RX_flag1 = 1; } } } /////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////// void init_PWM() { servo_cmd.period_ms(20); servo_cmd.write(servo_duty); pwm1.period_us(50); pwm1.write(0.5); TIM1->CCER |= 0x4; pwm2.period_us(50); pwm2.write(0.5); TIM1->CCER |= 0x40; } void init_CN() { HallA.rise(&CN_ITR); HallA.fall(&CN_ITR); HallB.rise(&CN_ITR); HallB.fall(&CN_ITR); HallA_2.rise(&CN_ITR); HallA_2.fall(&CN_ITR); HallB_2.rise(&CN_ITR); HallB_2.fall(&CN_ITR); } void CN_ITR() { // motor1 HallA_1_state = HallA.read(); HallB_1_state = HallB.read(); //led1 != led1; ///code for state determination/// ////////////////////////////////// ////////////////////////////////// //determine the state if((HallA_1_state == 0)&&(HallB_1_state == 0)) { state_1 = 1; } else if((HallA_1_state == 0)&&(HallB_1_state == 1)) { state_1 = 2; } else if((HallA_1_state == 1)&&(HallB_1_state == 1)) { state_1 = 3; } else if((HallA_1_state == 1)&&(HallB_1_state ==0)) { state_1 = 4; } //forward or backward int direction_1 = 0; direction_1 = state_1 - state_1_old; if((direction_1 == -1) || (direction_1 == 3)) { //forward speed_count_1 = speed_count_1 - 1; } else if((direction_1 == 1) || (direction_1 == -3)) { //backward speed_count_1 = speed_count_1 + 1; } else { //prevent initializing error state_1_old = state_1; } //change old state state_1_old = state_1; ////////////////////////////////// ////////////////////////////////// //forward : speed_count_1 + 1 //backward : speed_count_1 - 1 // motor2 HallA_2_state = HallA_2.read(); HallB_2_state = HallB_2.read(); ///code for state determination/// ////////////////////////////////// ///////////////////////////////// //determine the state if((HallA_2_state == 0)&&(HallB_2_state == 0)) { state_2 = 1; } else if((HallA_2_state == 0)&&(HallB_2_state == 1)) { state_2 = 2; } else if((HallA_2_state == 1)&&(HallB_2_state == 1)) { state_2 = 3; } else if((HallA_2_state == 1)&&(HallB_2_state ==0)) { state_2 = 4; } //forward or backward int direction_2 = 0; direction_2 = state_2 - state_2_old; if((direction_2 == 1) || (direction_2 == -3)) { //forward speed_count_2 = speed_count_2 - 1; } else if((direction_2 == -1) || (direction_2 == 3)) { //backward speed_count_2 = speed_count_2 + 1; } else { //prevent initializing error state_2_old = state_2; } //change old state state_2_old = state_2; ////////////////////////////////// ////////////////////////////////// //forward : speed_count_2 + 1 //backward : speed_count_2 - 1 }