Diff: driveFunc.cpp

Revision:

5:dfea493f7a12

Child:

6:a652deaae134

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/driveFunc.cpp	Sun Mar 11 13:15:34 2018 +0000
@@ -0,0 +1,36 @@
+#include <mbed.h>
+#include <funcdef.h>
+
+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;
+    
+    //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);
+    }     
+    
+    leftMotorVal.pulsewidth_us(leftMotorValTemp); 
+    rightMotorVal.pulsewidth_us(rightMotorValTemp); 
+  
+    return;  
+}
\ No newline at end of file