work
Dependencies: LSM9DS0 PID QEI mbed
main.cpp
- Committer:
- roger5641
- Date:
- 2016-09-23
- Revision:
- 0:cb8ba5eb9f7e
File content as of revision 0:cb8ba5eb9f7e:
#include "mbed.h" #include "QEI.h" #include "PID.h" Serial bluetooth(D10, D2); //uart for bluretooth Serial pc(D1, D0); //uart for pc QEI wheel_L (A1, A2, NC, 1560, 10, 0.01, QEI::X4_ENCODING); //390*4 = 1560 //chang array 50 to 10 QEI wheel_R (D13, D12, NC, 1560, 10, 0.01, QEI::X4_ENCODING);//390*4 = 1560 //chang array 50 to 10 PID speed_L_PID(0.1, 0.1, 0.0, 0.01);//0.00001, 0.00001, 0,,,//0.08, 0.08, 0.01, 0.01, 0.02 PID speed_R_PID(0.07, 0.07, 0.0, 0.01);//(0.1, 0.000005, 0, 0.01 //0.22, 0.11, 0.005, 0.01 PwmOut motor_L_pwm(D7); PwmOut motor_L_pwmn(D11); PwmOut motor_R_pwm(D8); PwmOut motor_R_pwmn(A3); Ticker timer1; void timer1_interrupt(void); void init_PWM(void); void init_TIMER(void); void init_sensor(void); void real_sensor_value(void); void sensor_fusion(void); float lpf(float input, float output_old, float frequency); float motor_L_AngularSpeed; float motor_R_AngularSpeed; float sample_speed = 0.0; float pwm1_duty = 0.5f; float pwm2_duty = 0.5f; char Idle_Mode = 'a'; char robot_Mode = 'a'; char speed_2 = 'b'; char speed_3 = 'c'; char speed_4 = 'd'; char speed_5 = 'e'; char speed_6 = 'f'; char speed_7 = 'g'; char speed_8 = 'h'; int main() { pc.baud(115200); bluetooth.baud(115200); init_PWM(); init_TIMER(); while(1) { ; } } void timer1_interrupt(void) { wheel_L.Calculate(); wheel_R.Calculate(); if(bluetooth.readable()) { robot_Mode = bluetooth.getc(); } if(robot_Mode == Idle_Mode) { //bluetooth.printf("Idle_Mode\n"); pwm1_duty = 0.5f; pwm2_duty = 0.5f; } else if(robot_Mode == speed_2) { //bluetooth.printf("speed_2\n"); sample_speed = 2.0; speed_L_PID.Compute(-sample_speed, wheel_L.getAngularSpeed()); speed_R_PID.Compute(sample_speed, wheel_R.getAngularSpeed()); pwm1_duty = 0.5f + speed_L_PID.output; pwm2_duty = 0.5f + speed_R_PID.output; } else if(robot_Mode == speed_3) { //bluetooth.printf("speed_3\n"); sample_speed = 3.0; speed_L_PID.Compute(sample_speed, wheel_L.getAngularSpeed()); speed_R_PID.Compute(-sample_speed, wheel_R.getAngularSpeed()); pwm1_duty = 0.5f + speed_L_PID.output; pwm2_duty = 0.5f + speed_R_PID.output; } else if(robot_Mode == speed_4) { //bluetooth.printf("speed_4\n"); sample_speed = 4.0; speed_L_PID.Compute(-sample_speed, wheel_L.getAngularSpeed()); speed_R_PID.Compute(sample_speed, wheel_R.getAngularSpeed()); pwm1_duty = 0.5f + speed_L_PID.output; pwm2_duty = 0.5f + speed_R_PID.output; } else if(robot_Mode == speed_5) { //bluetooth.printf("speed_5\n"); sample_speed = 5.0; speed_L_PID.Compute(sample_speed, wheel_L.getAngularSpeed()); speed_R_PID.Compute(-sample_speed, wheel_R.getAngularSpeed()); pwm1_duty = 0.5f + speed_L_PID.output; pwm2_duty = 0.5f + speed_R_PID.output; } else if(robot_Mode == speed_6) { //bluetooth.printf("speed_6\n"); sample_speed = 6.0; speed_L_PID.Compute(-sample_speed, wheel_L.getAngularSpeed()); speed_R_PID.Compute(sample_speed, wheel_R.getAngularSpeed()); pwm1_duty = 0.5f + speed_L_PID.output; pwm2_duty = 0.5f + speed_R_PID.output; } else if(robot_Mode == speed_7) { //bluetooth.printf("speed_7\n"); sample_speed = 7.0; speed_L_PID.Compute(sample_speed, wheel_L.getAngularSpeed()); speed_R_PID.Compute(-sample_speed, wheel_R.getAngularSpeed()); pwm1_duty = 0.5f + speed_L_PID.output; pwm2_duty = 0.5f + speed_R_PID.output; } else if(robot_Mode == speed_8) { //bluetooth.printf("speed_8\n"); sample_speed = 8.0; speed_L_PID.Compute(-sample_speed, wheel_L.getAngularSpeed()); speed_R_PID.Compute(sample_speed, wheel_R.getAngularSpeed()); pwm1_duty = 0.5f + speed_L_PID.output; pwm2_duty = 0.5f + speed_R_PID.output; } else { pwm1_duty = 0.5f; pwm2_duty = 0.5f; } motor_L_pwm.write(pwm1_duty); //>0.5 forward(-) ; <0.5 backward(+) motor_R_pwm.write(pwm2_duty); // <0.5 forward(-) ; >0.5 backward(+) TIM1->CCER |= 4; //enable ch1 complimentary output TIM1->CCER |= 64; //enable ch2 complimentary output pc.printf("motor_L_AngularSpeed: %f motor_R_AngularSpeed: %f \r\n",wheel_L.getAngularSpeed(), wheel_R.getAngularSpeed()); } void init_TIMER(void) { timer1.attach_us(&timer1_interrupt, 10000);//10ms interrupt period (100 Hz) } void init_PWM(void) { motor_L_pwm.period_us(50); motor_L_pwm.write(0.5f); motor_R_pwm.period_us(50); motor_R_pwm.write(0.5f); TIM1->CCER |= 4; TIM1->CCER |= 64; speed_L_PID.SetOutputLimits(0.5, -0.5); speed_R_PID.SetOutputLimits(0.5, -0.5); }