Diff: motor.cpp

Branch:

motorupdate

Revision:

24:15c6bbdbb0e4

Parent:

23:96c36a38d83b

Child:

25:255d169a45e1

--- a/motor.cpp	Wed Dec 07 16:00:30 2016 +0000
+++ b/motor.cpp	Wed Jan 11 10:53:26 2017 +0000
@@ -135,26 +135,31 @@
         
 void sensorCorner(float &w1,float &w2,float theta,float speed)
 {   // when cornering left the left motor slows down more than the right hand side
-    // may just replace with ACC and DECC
+   
    
-    bool leftOrRight;
-    
+    bool leftOrRight = false;
+    float tune = 1.2;
     if(theta<0)
         {
             leftOrRight=true;
             theta=theta*-1;   
             
         }
- 
+        
+    if(theta>0.5)
+    {
+     theta = 0.5;   
+        
+    }
     float deltaW = ((0.1f*tan((theta/0.02222f)* (3.14f / 180.0f)))/0.2f)*speed;
     
     if(leftOrRight){
-          w1= speed+ deltaW;
-          w2= speed -deltaW;
+          w1= speed+ deltaW * tune;
+          w2= speed -deltaW * tune;
           }
     else{
-            w1= speed- deltaW;
-            w2= speed +deltaW;
+            w1= speed- deltaW*tune;
+            w2= speed +deltaW*tune;
         }