File content as of revision 1:f4e3365155e1:

void ResetDistanceTimeSpeed() //reset vaviables for new straight or turn
{
    distanceA=0;
    distanceB=0;
    speedA=0;
    speedB=0;
    timer.reset();
    timer.start();
}

void stopmotors() //stop motors dead
{
    PWMA.write(0.0f);       //0% duty cycle
    PWMB.write(0.0f);       //0% duty cycle
}

void HuntSpeeds(float target_speedA, float target_speedB) //alter speeds to desired speed by hunting for corresponding duty (input desired speeds)
{

    float deltaA = target_speedA -speedA; //find difference between actual speed and desired speed
    dutyA = dutyA + deltaA*0.1f; //modify duty dependant upon size of error
    PWMA.write(dutyA); //write new duty to motor

    float deltaB = target_speedB -speedB; //repeat for B motor
    dutyB = dutyB + deltaB*0.1f;
    PWMB.write(dutyB);
}


int forward(float distance, float speed) //forward motion, input distance and speed (rev/s)
{
    // Set motor direction forward
    DIRA = FORWARD;
    DIRB = FORWARD;

    //reset distance
    ResetDistanceTimeSpeed();

    // Set initial motor to be altered by speed code
    PWMA.write(0.1f*speed);          //Set duty cycle (%)
    PWMB.write(0.1*speed);          //Set duty cycle (%)


    //wait for run to complete byy checking if distance has been covered yet
    while (((distanceA+distanceB)/2) < distance) {
        //maintain desired speed
        HuntSpeeds(speed, speed);
    }
    return 1;
}

int turn(float degrees, float speed, int direction,float turn_radius) // (Degrees of Turn, Speed, (Anti/Clock)wise, turn radius (m))

{
    // Calculate Turn Distance
    float C1 = (pi * 2 * turn_radius)*(degrees/360); // outside arc (piD)/fraction turn
    float C2 = (pi * 2 * (turn_radius-wheel_spacing))*(degrees/360); //inside arc (piD)/fraction turn
    //Set Initial Motor Direction
    DIRA = FORWARD;
    DIRB = FORWARD;

    // Test for direction to Enter Victory Spin
    if(direction==REVERSE) {
        DIRA = REVERSE;
        DIRB = FORWARD;//victory spin opposite wheels
    }
    // Reset Distance Travelled
    ResetDistanceTimeSpeed();

    // Wait for Turn to Complete by checking whether outer wheel completed spin yet
    while (distanceA < C1) {
        if(direction==FORWARD) {
            HuntSpeeds(speed, speed*(float(C2/C1))); //maitain speeds proportional to difference in turn length for each wheel
        }

        if(direction==REVERSE) {
            HuntSpeeds(speed,speed); //set speeds same for reverse spin
        }
    }
    return 1;
}