Diff: driveFunc.cpp

Revision:

6:a652deaae134

Parent:

5:dfea493f7a12

Child:

7:11dd5581c763

--- a/driveFunc.cpp	Sun Mar 11 13:15:34 2018 +0000
+++ b/driveFunc.cpp	Sun Apr 01 18:21:50 2018 +0000
@@ -4,33 +4,40 @@
 void driveFunc()
 {
     //USE TEMPORARY VARIABLES TO WORK OUT A PULSE LENGTH FOR THE MOTORS, DICTATED BY ERROR OFFSET
-    int leftMotorValTemp=initialMotorspeed-PIDs;
-    int rightMotorValTemp=initialMotorspeed+PIDs;
+    leftMotorValTemp=initialMotorspeed-newPIDs;
+    rightMotorValTemp=initialMotorspeed+newPIDs;
     
+    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)
     {
         leftMotorValTemp=1000;
-        //beep(1);
     }    
     if (rightMotorValTemp>1000)
     {
         rightMotorValTemp=1000;
-        //beep(1);
     }    
     if (leftMotorValTemp<0)
     {
         leftMotorValTemp=0;
-        //beep(1);
     }
     if (rightMotorValTemp<0)
     {
         rightMotorValTemp=0;
-        //beep(1);
     }     
-    
+    if (!stopMotors)
+    {   
     leftMotorVal.pulsewidth_us(leftMotorValTemp); 
-    rightMotorVal.pulsewidth_us(rightMotorValTemp); 
+    rightMotorVal.pulsewidth_us(rightMotorValTemp);
+    }
+    else
+    {
+        leftMotorVal.pulsewidth_us(0); 
+        rightMotorVal.pulsewidth_us(0);
+        for(;;){}
+    } 
   
     return;  
 }
\ No newline at end of file