File content as of revision 6:a652deaae134:

#include <mbed.h>
#include <funcdef.h>

void driveFunc()
{
    //USE TEMPORARY VARIABLES TO WORK OUT A PULSE LENGTH FOR THE MOTORS, DICTATED BY ERROR OFFSET
    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;
    }    
    if (rightMotorValTemp>1000)
    {
        rightMotorValTemp=1000;
    }    
    if (leftMotorValTemp<0)
    {
        leftMotorValTemp=0;
    }
    if (rightMotorValTemp<0)
    {
        rightMotorValTemp=0;
    }     
    if (!stopMotors)
    {   
    leftMotorVal.pulsewidth_us(leftMotorValTemp); 
    rightMotorVal.pulsewidth_us(rightMotorValTemp);
    }
    else
    {
        leftMotorVal.pulsewidth_us(0); 
        rightMotorVal.pulsewidth_us(0);
        for(;;){}
    } 
  
    return;  
}