Diff: main.cpp

Revision:

3:e4bdbe286f7a

Parent:

2:dc39c3de56c1

--- a/main.cpp	Fri May 27 02:37:20 2016 +0000
+++ b/main.cpp	Fri Jun 10 14:32:13 2016 +0000
@@ -3,9 +3,8 @@
 #define Ts 0.01f    //period of timer1 (s)
 #define Kp 0.006f
 #define Ki 0.02f
-#define k1 1.02f
-#define k2 1.02f
 
+PwmOut mypwm(A0);
 PwmOut pwm1(D7);
 PwmOut pwm1n(D11);
 PwmOut pwm2(D8);
@@ -53,6 +52,8 @@
 float v2 = 0.0, v2_ref = 0.0;
 float v2_err = 0.0, v2_ierr = 0.0, PIout_2 = 0.0, PIout_2_old = 0.0;
  
+float k1 = 0.0, k2 = 0.0, servo_duty = 0.121;
+
 int main() 
 {
     init_TIMER();
@@ -81,9 +82,23 @@
             else 
                 Degree_Error=Degree_Error;
                 
-            Command_Flag = 0;
+            if(Receive_Data[9]=='j')
+                mypwm.write(0.079);
+            else 
+                mypwm.write(0.1257);
+                          
             
-  
+            if  (Position_Error >= 12 || Position_Error <= -12)
+                k1 = 2;
+            else 
+                k1 = 1;
+            
+            if  (Degree_Error >= 15 || Degree_Error <= -15)
+                k2 = 1;
+            else
+                k2 = 0.5;
+            
+            
             sigma = k1*Position_Error;
             delta = k2*Degree_Error;
             
@@ -91,10 +106,14 @@
             if (sigma < -500){sigma = -500;}
             if (delta > 500){delta = 500;}
             if (delta < -500){delta = -500;}
+            
+            
             fSpeedRef_1 = sigma + delta;
             fSpeedRef_2 = sigma - delta;
             v1_ref = fSpeedRef_1;
             v2_ref = -1*fSpeedRef_2;
+            
+            Command_Flag = 0;
         }
     }
 }
@@ -202,6 +221,9 @@
     pwm2.period_us(50);
     pwm2.write(0.5);
     TIM1->CCER |= 0x40;
+    
+    mypwm.period_ms(20);
+    mypwm.write(servo_duty);
 }
  
 void init_CN(void)
@@ -224,27 +246,30 @@
 
 void BT_interrupt(void)
 {  
-    Temp = bluetooth.getc(); 
-    
-    if (Receive_Flag==1)
+    if(bluetooth.readable())
     {
-        Receive_Counter++;
-        Receive_Data[Receive_Counter]=Temp;
-     
-        if(Receive_Counter==9)
+        Temp = bluetooth.getc(); 
+        
+        if (Receive_Flag==1)
         {
-            Command_Flag=1;
-            Receive_Flag=0;
-            Receive_Counter=0;
+            Receive_Counter++;
+            Receive_Data[Receive_Counter]=Temp;
+         
+            if(Receive_Counter==9)
+            {
+                Command_Flag=1;
+                Receive_Flag=0;
+                Receive_Counter=0;
+            }
         }
-    }
-    else
-    {
-        if(Temp == 36)
+        else
         {
-            Receive_Flag = 1;
-            Receive_Counter = 0;
-            Receive_Data[0]= Temp;
+            if(Temp == 36)
+            {
+                Receive_Flag = 1;
+                Receive_Counter = 0;
+                Receive_Data[0]= Temp;
+            }
         }
     }
 }