![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
work
Dependencies: LSM9DS0 PID QEI mbed
Revision 0:cb8ba5eb9f7e, committed 2016-09-23
- Comitter:
- roger5641
- Date:
- Fri Sep 23 09:38:27 2016 +0000
- Commit message:
- work
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/LSM9DS0.lib Fri Sep 23 09:38:27 2016 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/teams/LDSC_Robotics_TAs/code/LSM9DS0/#fb89056ade7f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID.lib Fri Sep 23 09:38:27 2016 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/teams/LDSC_Robotics_TAs/code/PID/#4df4895863cd
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/QEI.lib Fri Sep 23 09:38:27 2016 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/teams/LDSC_Robotics_TAs/code/QEI/#a5aa3e6ea2b7
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Sep 23 09:38:27 2016 +0000 @@ -0,0 +1,197 @@ +#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); +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Fri Sep 23 09:38:27 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/2241e3a39974 \ No newline at end of file