![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
speed
Dependencies: mbed
Diff: main.cpp
- Revision:
- 0:95df37b1dd11
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Sat May 28 12:50:40 2016 +0000 @@ -0,0 +1,277 @@ +/*DCMotor*/ +// transfer function 10400/s+21.28 +// motor direction v1(vR) < 0 +// motor direction v2(vL) > 0 +// speed limitatino 300 rpm + +#include "mbed.h" + +//The number will be compiled as type "double" in default +//Add a "f" after the number can make it compiled as type "float" +#define Ts 0.02f //period of timer1 (s) +#define Kp1 0.0048f //0.0048f +#define Kp2 0.0048f //0.0048f +#define Ki 0.002754f //0.1023f + +Serial bluetooth(D10,D2); //宣告藍牙腳位 +//Serial pc(D1, D0); +PwmOut servo(A0); +PwmOut pwm1(D7); +PwmOut pwm1n(D11); +PwmOut pwm2(D8); +PwmOut pwm2n(A3); + +DigitalOut led1(A4); +DigitalOut led2(A5); + +//Motor1 sensor +InterruptIn HallA_1(A1); +InterruptIn HallB_1(A2); +//Motor2 sensor +InterruptIn HallA_2(D13); +InterruptIn HallB_2(D12); + + +Ticker timer1; +void timer1_interrupt(void); +int timer1_counter; + +void CN_interrupt(void); + +void init_TIMER(void); +void init_PWM(void); +void init_CN(void); +void init_BLUETOOTH(void); + +int8_t stateA_1=0, stateB_1=0, stateA_2=0, stateB_2=0; +int8_t state_1 = 0, state_1_old = 0, state_2 = 0, state_2_old = 0; + +int v1Count = 0; +int v2Count = 0; + +float v1 = 0.0, v1_ref = 0.0; +float v1_err = 0.0, v1_err_old = 0.0, PIout_1 = 0.0; +float v1_old[10] = {}, v1_avg = 0.0; +float v2 = 0.0, v2_ref = 0.0; +float v2_err = 0.0, v2_err_old = 0.0, PIout_2 = 0.0; +float v2_old[10] = {}, v2_avg = 0.0; + +int servo_angle = 87; +float servo_duty = 0.079;//0.079 +(0.084/180)*angle, -90<angle<90 + +char Receive_Data[8] = {}; + +int main() { + init_BLUETOOTH(); + init_TIMER(); + init_PWM(); + init_CN(); + + v1_ref = 0.0; + v2_ref = 0.0; + + //bluetooth.baud(115200); //設定鮑率 + //pc.baud(57600); + + while(1) + { + if(bluetooth.readable()) + { + bluetooth.scanf("%f%f",&v1_ref,&v2_ref); + v1_ref = v1_ref - 300.0f; + v2_ref = v2_ref - 300.0f; + } + /*if(pc.readable()) + { + bluetooth.putc(pc.getc()); + } + if(bluetooth.readable()) + { + pc.putc(bluetooth.getc()); + }*/ + } +} + +void timer1_interrupt(void) +{ + + /*for(int i=0; i<8; i++) + { + Receive_Data[i] = bluetooth.getc(); + } + //read data from matlab + //distance_target + v1_ref = (Receive_Data[1]-0x30)*100 + (Receive_Data[2]-0x30)*10 + (Receive_Data[3]-0x30); + + if(Receive_Data[0] == '-')v1_ref = -1* v1_ref; + else v1_ref = v1_ref; + + //ang_rel_target + v2_ref = (Receive_Data[5]-0x30)*100 + (Receive_Data[6]-0x30)*10 + (Receive_Data[7]-0x30); + + if(Receive_Data[4] == '-')v2_ref = -1* v2_ref; + else v2_ref = v2_ref;*/ + + //Motor 1 + v1 = (float)v1Count * 50.0f / 12.0f * 60.0f / 29.0f; //unit: rpm + v1_avg = v1_avg + ( v1 - v1_old[9])/10.0f; + for(int i = 9; i > 0 ; i--) + { + v1_old[i] = v1_old[i-1]; + } + v1_old[0] = v1; + v1Count = 0; + + ///code for PI control/// + v1_err = v1_ref - v1; + + PIout_1 = PIout_1 + Kp1 * v1_err - Ki * v1_err_old; + v1_err_old = v1_err; + + // saturation + if(PIout_1 >= 0.5f)PIout_1 = 0.5f; + else if(PIout_1 <= -0.5f)PIout_1 = -0.5f; + pwm1.write(PIout_1 + 0.5f); + TIM1->CCER |= 0x4; + + //Motor 2 + v2 = (float)v2Count * 50.0f / 12.0f * 60.0f / 29.0f; //unit: rpm + v2_avg = v2_avg + ( v2 - v2_old[9])/10.0f; + for(int i = 9; i > 0; i--) + { + v2_old[i] = v2_old[i-1]; + } + v2_old[0] = v2; + v2Count = 0; + + ///code for PI control/// + v2_err = v2_ref - v2; + + PIout_2 = PIout_2 + Kp2 * v2_err - Ki * v2_err_old; + v2_err_old = v2_err; + + //saturation + if(PIout_2 >= 0.5f)PIout_2 = 0.5f; + else if(PIout_2 <= -0.5f)PIout_2 = -0.5f; + pwm2.write(PIout_2 + 0.5f); + TIM1->CCER |= 0x40; + + timer1_counter ++; + if (timer1_counter == 50) + { + timer1_counter = 0; + if(bluetooth.writeable()) + { + bluetooth.printf("V1: %4.2f V2: %4.2f\n",v1_avg,v2_avg); + } + } + + /*servo_duty = 0.079 + (0.084/180)*servo_angle; // 要修改 + servo.write(servo_duty); + servo = 1; + wait(0.1); + servo = 0; + */ +} + +void CN_interrupt(void) +{ + //Motor 1 + stateA_1 = HallA_1.read(); + stateB_1 = HallB_1.read(); + + ///code for state determination/// + if (stateA_1 == 0) + { + if (stateB_1 == 0) + state_1 = 0; + else + state_1 = 1; + } + else + { + if (stateB_1 == 1) + state_1 = 2; + else + state_1 = 3; + } + + //Forward: v1Count +1 + //Inverse: v1Count -1 + if ( (state_1 == (state_1_old + 1)) || (state_1 == 0 && state_1_old == 3) ) + v1Count++; + else if ( (state_1 == (state_1_old - 1)) || (state_1 == 3 && state_1_old == 0)) + v1Count--; + + state_1_old = state_1; + + //Motor 2 + stateA_2 = HallA_2.read(); + stateB_2 = HallB_2.read(); + + ///code for state determination/// + if (stateA_2 == 0) + { + if (stateB_2 == 0) + state_2 = 0; + else + state_2 = 1; + } + else + { + if (stateB_2 == 1) + state_2 = 2; + else + state_2 = 3; + } + + //Forward: v2Count +1 + //Inverse: v2Count -1 + if ( (state_2 == (state_2_old + 1)) || (state_2 == 0 && state_2_old == 3) ) + v2Count++; + else if ( (state_2 == (state_2_old - 1)) || (state_2 == 3 && state_2_old == 0) ) + v2Count--; + + state_2_old = state_2; + + +} + +void init_TIMER(void) +{ + timer1.attach_us(&timer1_interrupt, 20000);//10ms interrupt period (100 Hz) + timer1_counter = 0; +} + +void init_PWM(void) +{ + 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(void) +{ + HallA_1.rise(&CN_interrupt); + HallA_1.fall(&CN_interrupt); + HallB_1.rise(&CN_interrupt); + HallB_1.fall(&CN_interrupt); + + HallA_2.rise(&CN_interrupt); + HallA_2.fall(&CN_interrupt); + HallB_2.rise(&CN_interrupt); + HallB_2.fall(&CN_interrupt); + + stateA_1 = HallA_1.read(); + stateB_1 = HallB_1.read(); + stateA_2 = HallA_2.read(); + stateB_2 = HallB_2.read(); +} +void init_BLUETOOTH(void) +{ + bluetooth.baud(115200); +} \ No newline at end of file