Diff: driveFunc.cpp

Revision:

7:11dd5581c763

Parent:

6:a652deaae134

--- a/driveFunc.cpp	Sun Apr 01 18:21:50 2018 +0000
+++ b/driveFunc.cpp	Sat Apr 28 21:34:55 2018 +0000
@@ -7,17 +7,17 @@
     leftMotorValTemp=initialMotorspeed-newPIDs;
     rightMotorValTemp=initialMotorspeed+newPIDs;
     
-    rightMotorValTemp=53.9*exp(2.89*0.001*rightMotorValTemp); 
-    leftMotorValTemp=53.9*exp(2.89*0.001*leftMotorValTemp); 
+    //rightMotorValTemp=53.9*exp(2.89*0.001*rightMotorValTemp); 
+    //leftMotorValTemp=53.9*exp(2.89*0.001*leftMotorValTemp); 
  
     //IF THE MOTOR SPEED GOES OUTSIDE POSSIBLE BOUNDARIES, CLAMP
-    if (leftMotorValTemp>1000)
+    if (leftMotorValTemp>=1000)
     {
-        leftMotorValTemp=1000;
+        leftMotorValTemp=999;
     }    
-    if (rightMotorValTemp>1000)
+    if (rightMotorValTemp>=1000)
     {
-        rightMotorValTemp=1000;
+        rightMotorValTemp=999;
     }    
     if (leftMotorValTemp<0)
     {
@@ -27,17 +27,36 @@
     {
         rightMotorValTemp=0;
     }     
+    
+    for (int i=0;i<=10;i++)
+    {
+        if ((leftMotorValTemp>=motorLookup[i][0]) && (leftMotorValTemp<motorLookup[i+1][0]))
+        {
+            leftMotorValTemp=motorLookup[i][1]+(leftMotorValTemp-motorLookup[i][0])*((motorLookup[i+1][1]-motorLookup[i][1])/(motorLookup[i+1][0]-motorLookup[i][0]));
+            break;
+        }
+    }
+    
+    for (int i=0;i<=10;i++)
+    {
+        if ((rightMotorValTemp>=motorLookup[i][0]) && (rightMotorValTemp<motorLookup[i+1][0]))
+        {
+            rightMotorValTemp=motorLookup[i][1]+(rightMotorValTemp-motorLookup[i][0])*((motorLookup[i+1][1]-motorLookup[i][1])/(motorLookup[i+1][0]-motorLookup[i][0]));
+            break;
+        }
+    }
+    
     if (!stopMotors)
     {   
-    leftMotorVal.pulsewidth_us(leftMotorValTemp); 
-    rightMotorVal.pulsewidth_us(rightMotorValTemp);
+        leftMotorVal.pulsewidth_us(leftMotorValTemp); 
+        rightMotorVal.pulsewidth_us(rightMotorValTemp);
     }
     else
     {
-        leftMotorVal.pulsewidth_us(0); 
-        rightMotorVal.pulsewidth_us(0);
-        for(;;){}
+        //leftMotorVal.pulsewidth_us(0); 
+        //rightMotorVal.pulsewidth_us(0);
+        //for(;;){}
     } 
-  
+
     return;  
 }
\ No newline at end of file