Roger Weng
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
diff -r 000000000000 -r cb8ba5eb9f7e LSM9DS0.lib --- /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
diff -r 000000000000 -r cb8ba5eb9f7e PID.lib --- /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
diff -r 000000000000 -r cb8ba5eb9f7e QEI.lib --- /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
diff -r 000000000000 -r cb8ba5eb9f7e main.cpp
--- /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);
+}
diff -r 000000000000 -r cb8ba5eb9f7e mbed.bld --- /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