work

Dependencies:   LSM9DS0 PID QEI mbed

Files at this revision

API Documentation at this revision

Comitter:
roger5641
Date:
Fri Sep 23 09:38:27 2016 +0000
Commit message:
work

Changed in this revision

LSM9DS0.lib Show annotated file Show diff for this revision Revisions of this file
PID.lib Show annotated file Show diff for this revision Revisions of this file
QEI.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
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