Diff: main.cpp

Revision:

1:8fb27fda4ec5

Parent:

0:81b42d622dc5

Child:

2:dc39c3de56c1

--- a/main.cpp	Thu May 26 12:30:18 2016 +0000
+++ b/main.cpp	Thu May 26 12:54:23 2016 +0000
@@ -61,42 +61,38 @@
     
     bluetooth.baud(115200);
     pc.baud(57600);
+    bluetooth.attach(&BT_interrupt);
     
     while(1)    
     {
-        if(bluetooth.readable())
+        if(Command_Flag==1)
         {
-            pc.attach(&BT_interrupt);
-
-            if(Command_Flag==1)
-            {
-                Position_Error=(Receive_Data[2]-0x30)*100+(Receive_Data[3]-0x30)*10+(Receive_Data[4]-0x30); 
-                
-                if(Receive_Data[1]=='-')
-                    Position_Error=-1*Position_Error;
-                else 
-                    Position_Error=Position_Error; 
-                    
-                Degree_Error=(Receive_Data[6]-0x30)*100+(Receive_Data[7]-0x30)*10+(Receive_Data[8]-0x30);
+            Position_Error=(Receive_Data[2]-0x30)*100+(Receive_Data[3]-0x30)*10+(Receive_Data[4]-0x30); 
+            
+            if(Receive_Data[1]=='-')
+                Position_Error=-1*Position_Error;
+            else 
+                Position_Error=Position_Error; 
                 
-                if(Receive_Data[5]=='-')
-                    Degree_Error=-1*Degree_Error;
-                else 
-                    Degree_Error=Degree_Error;
-                
-      
-                sigma = k1*Position_Error;
-                delta = k2*Degree_Error;
-                
-                if (sigma > 500){sigma = 500;}
-                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;
-            }
+            Degree_Error=(Receive_Data[6]-0x30)*100+(Receive_Data[7]-0x30)*10+(Receive_Data[8]-0x30);
+            
+            if(Receive_Data[5]=='-')
+                Degree_Error=-1*Degree_Error;
+            else 
+                Degree_Error=Degree_Error;
+            
+  
+            sigma = k1*Position_Error;
+            delta = k2*Degree_Error;
+            
+            if (sigma > 500){sigma = 500;}
+            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;
         }
     }
 }