Diff: motor.cpp

Branch:

testing

Revision:

11:4a6f97cc1f1e

Parent:

10:f4fc8ccde4ad

--- a/motor.cpp	Fri Nov 11 12:50:48 2016 +0000
+++ b/motor.cpp	Mon Nov 14 18:04:20 2016 +0000
@@ -16,11 +16,11 @@
 
 //Corner Control
 
-
-void corner(float &w1,float &w2,float deltaTheta,int maxspeed)
+void dutyCycleCorner( float speed, float angle);
+void corner(float &w1,float &w2,float deltaTheta,int maxspeed);
 
 void steering(float center, float theta, int maxspeed);
-
+void centerWheels();
 int cornerPwmControl;// this sets the cornering percentage ratio. value between 1 and 0.
                      // 100 means both motors run at the same speed. 0 means that one motor turns off.
 
@@ -139,14 +139,14 @@
     }
     
     
-void speedSetting(int w1, int w2 ,int w1M, int w2M)
+/*void speedSetting(int w1, int w2 ,int w1M, int w2M)
 {
     // need to compare the measured frequency 
     
     float deltaW1 = w1 - w1M;
     float deltaW2 = w2 - w2M;
     
-    if(delatW1 <0)
+    if(deltaW1 <0)
     {
         changespeed(0,w1);   
     }
@@ -156,7 +156,7 @@
         changespeed(1,w1);
     } 
     return;
-}
+}*/
     
 //need to fill out function for calculating the change in speed.
 void changespeed(bool a, float w)
@@ -182,16 +182,38 @@
     
     
     
-// this is a function whihc works off of the duty cycle.
-void dutyCycleCorner( float speed,float & w1, float & w2,float  middlePoint)
+// this is a function whihc works off of the duty cycle. NO SENSOR REQUIREMENT
+void dutyCycleCorner( float speed, float angle)
 {
-        float width;
+        
+        //float width =0.15;
         //float radius= center(middlePoint);
-        float radius = 0.6;
-        float w1=(speed/radius)*(radius + width/2);
-        float w2=(speed/radius)*(radius - width/2);
+        
+        bool leftOrRight;
+        float theta = angle/0.02222f;
+        if(theta<0)
+        {
+         leftOrRight=true;
+         theta=theta*-1;   
+            
+        }
+//       float diff = (tan(theta))/ 2; 
+       
+       
         
-        TFC_
+        //float radius = 0.6;
+        float x = tan(theta* (3.14f / 180.0f));
+        float deltaW = ((0.2f*x)/0.2f)*speed;
+        float w1=speed+ deltaW;
+        float w2= speed- deltaW;
+        
+        TFC_SetMotorPWM(w2,w1);
+        /*if(leftOrRight){
+        TFC_SetMotorPWM(w1,w2);
+        }
+        else{
+            TFC_SetMotorPWM(w2,w1);
+        }*/
         
         
         
@@ -205,11 +227,13 @@
     float r;
     float d;
     float l;
+    
+    
     // it may be worth doing this off the change in theta. so that it is insesnsitive to the calibration.
     float diff= ((d*tan(deltaTheta)/(2*l)));
     
-    float w1 = (maxspeed/r)*(1+diff);
-    float w2 = (maxspeed/r)*(1-diff);
+     w1 = (maxspeed/r)*(1+diff);
+     w2 = (maxspeed/r)*(1-diff);
     
     
         
@@ -241,10 +265,9 @@
         corner(w1,w2,deltaTheta,maxspeed);
     }
     
-    else()
-    {
+    else{
         //going right?
-        corner(w2,w1,deltaTheta);
+        corner(w2,w1,deltaTheta,maxspeed);
         
     }