File content as of revision 0:bcc2a42d221f:

#include "mbed.h"
#include "Motor.h"

//#define DEBUG 1

#define SENSOR_L    p30
#define SENSOR_R    p29
#define MOTOR_L_PWM p26
#define MOTOR_L_FWD p25
#define MOTOR_L_REV p24
#define MOTOR_R_PWM p21
#define MOTOR_R_FWD p23
#define MOTOR_R_REV p22
#define BASE_SPEED  0.4

DigitalOut myled(LED1);
DigitalIn leftSensor (SENSOR_L);
DigitalIn rightSensor(SENSOR_R);
Motor       leftMotor (MOTOR_L_PWM, MOTOR_L_FWD, MOTOR_L_REV);
Motor       rightMotor(MOTOR_R_PWM, MOTOR_R_FWD, MOTOR_R_REV);

void backup(void);
void rotate_left(void);
void rotate_right(void);
void turn_around(void);

int main() {
    //Set motors to forward
    leftMotor.speed(BASE_SPEED);
    rightMotor.speed(BASE_SPEED);
    
    while(1) {
        #ifdef DEBUG
        printf("Sensor Check:\r\n");
        printf("Left  %d\r\n", left_test.read());
        printf("Right %d\r\n", right_test.read());
        #endif
        

        if(leftSensor & rightSensor){
            backup();
            turn_around();
        }else if(leftSensor){
            backup();
            rotate_right();
        }else if(rightSensor){
            backup();
            rotate_left();
        }
        
        leftMotor.speed(BASE_SPEED);
        rightMotor.speed(BASE_SPEED);
        myled = 1;
        wait(0.2);
        myled = 0;
        wait(.1);
    }
}


void backup(void){
    leftMotor.speed (-1*BASE_SPEED);
    rightMotor.speed(-1*BASE_SPEED);
    wait(.5);
}
void rotate_left(void){
    leftMotor.speed (-1*BASE_SPEED);
    rightMotor.speed(BASE_SPEED);
    wait(1);
}
void rotate_right(void){
    leftMotor.speed(BASE_SPEED);
    rightMotor.speed(-1*BASE_SPEED);
    wait(1);
}
void turn_around(void){
    leftMotor.speed(BASE_SPEED);
    rightMotor.speed(-1*BASE_SPEED);
    wait(2);
}