File content as of revision 0:8fe745012832:

#include <mbed.h>
#include <motordriver.h>
#include <Servo.h>
#include <GP2xx.h>
//pc interface
Serial pc(USBTX, USBRX); // tx, rx
//servos
Servo LeftServo(p24);
Servo RightServo(p23);
//motors, left and right side
Motor leftM(p22, p6, p5, 1); // pwm, fwd, rev, can break
Motor rightM(p21, p7, p8, 1); // pwm, fwd, rev, can break
//range finders, left and right side, and left and right front
IRRangeFinder LS(p18,1);
IRRangeFinder LF(p17,1);
IRRangeFinder RF(p16,1);
IRRangeFinder RS(p15,1);
//debug leds.
DigitalOut led1 (LED1);
DigitalOut led2 (LED2);
DigitalOut led3 (LED3);
DigitalOut led4 (LED4);
DigitalOut ledleft  (p14);
DigitalOut ledright (p13);
DigitalOut ledfront (p12);
/** simple object avoidance using a logo based set of functions. it works and provides some intresting ways
* to control behaviour.
*
*/


/** function to spin the robot on the spot. technicaly redundant, as turn can do the same, but clearer when coding
* @param time, how long to spin for. 
* @param speed how fast to run the motors.
*
*/
float spin (float time, float speed);//-1 to 1
/** function to make the robot go fowards in a straight line
* @param time, how long to go straight for. 
* @param speed how fast to run the motors. -1 is reverse, 1 is foward, 0 is technicaly possible but means the robot coasts. 
*
*/
float straight (float time, float speed);//0 to 1
/** function to make the robot turn
* @param time, how long to turn for. 
* @param speed how fast to run the motors. -1 is right, 1 is left, 0 is technicaly possible but means the robot coasts. 
* @param diffrence, from 1 to -1. how much slower the opposite side is -1 is same speed and direction, 1 is same speed opposite direction
*/
float turn (float time, float speed, float diffrence);
/** function to make the robot stop
* @param time, how long to stop for 
* @param duty how hard to stop the motors.  1 is full on, 0 is technicaly possible but means the robot coasts. 
*
*/
float stop (float time, float duty);

int main() {
    float tempLF = 0, tempRF = 0;
    while (1) {
        tempLF = LS.read();//cuts down function calls, and therefore clock cycles used
        tempRF = RS.read();

        if (tempLF < 12) {
            if (tempLF > tempRF) {
                turn(0.1,-0.8,0.5);
            } else if (tempLF < tempRF) {
                turn(0.1,1,0.5);
            } else {
                spin(0.1,1);
            }
        } else if (tempRF < 12) {
            if (tempLF < tempRF) {
                turn(0.1,-1,0.5);
            } else if (tempLF > tempRF) {
                turn(0.1,1,0.5);
            } else {
               spin(0.1,1);
            }
        } else {
            straight(0.1, 0.7);
        }
    }
}

float spin (float time, float speed) {
    float temp1 = 0, temp2 = 0;
    temp1 = leftM.speed(speed);
    temp2 = rightM.speed(-speed);
    wait(time);
    leftM.coast():
    rightM.coast();
    return temp1+temp2;//should return 0.
}

float straight (float time, float speed) {
    float temp1 = 0, temp2 = 0;
    temp1 = leftM.speed(speed);
    temp2 = rightM.speed(speed);
    wait(time);
    leftM.coast():
    rightM.coast();
    return temp1-temp2;//should return 0.
}

float stop (float time, float duty) {
    float temp1 = 0, temp2 = 0;
    temp1 = leftM.stop(duty);
    temp2 = rightM.stop(duty);
    wait(time);
    leftM.coast():
    rightM.coast();
    return temp1-temp2;//should return 0.
}

float turn (float time, float speed, float diffrence) {
    float temp1 = -speed*diffrence;
    if (speed<0) {
        leftM.speed(speed);
        rightM.speed(temp1);
    } else if (speed>0) {
        leftM.speed(temp1);
        rightM.speed(speed);
    } else if (speed == 0) {
        leftM.coast();
        rightM.coast();
    } else {
        return -5;
    }
    wait(time);
    leftM.coast():
    rightM.coast();
    return temp1;//should return -speed*diffrence.
}