Diff: main.cpp

Revision:

0:cb8ba5eb9f7e

--- /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);
+}