File content as of revision 0:eff3ddf69354:

#include "mbed.h"
#include "Servo.h"
#include "SRF05.h"

Servo motor (p21);
Servo turn (p22);

SRF05 front (p24,p23);
SRF05 back (p26,p25);

DigitalOut b (p20);
DigitalOut led1 (LED1);
DigitalOut led2 (LED2);

void backward (float);
void forward (float);
void right (void);
void left (void);
void reset (void);
void stop (void);


int i = 0;
int j = 0;

float sample_delay = 0.01;

int main() {

    b = 0;
    reset();

    //main loop
    while (1) {

        //init
        i = front.read();
        j = back.read();

        
           //can go forward?
        if (i > 100) {
            while (i > 100) {

                forward(20);
                i = front.read();
                j = back.read();
            }

            stop();
        }
        
        //can it turn to avoid?
        if (i > 10 && i < 100) {
            led1 = 1;
            while (i > 10 && i < 100) {
                
                forward(20);
                turn.write(0);
                
                i = front.read();
                j = back.read();
            }
            led1 = 0;
            stop();
        }
        
        //is there room to ververse and turn?
        if (i < 20 && j > 10) {
        led2 = 1;
            while (i < 20 && j > 10) {
            
                backward(20);
                turn.write(1);
                
                i = front.read();
                j = back.read();
            }
            led2 = 0;
            stop();
        }
    }
}

void reset (void) {
    turn.write(0.5);
    motor.write(0.48);
}

// 0.5 = 0 speed & 0 = full speed
void forward (float speed) {
    float y = speed / 100;
    float x = y * 0.5;
    float z = 0.5 - x;

    motor.write(z);
}

// 0.5 = 0 speed & 1 = full speed

void backward (float speed) {
    float y = speed / 100;
    float x = y * 0.5;
    float z = 0.5 + x;

    motor.write(z);
}

// 0 = right 0.5 = straight 1 = left
void left (void) {
    turn.write(1);
}
void right (void) {
    turn.write(0);
}

void stop (void) {
    b = 1;
    wait(0.3);
    reset();
    wait(0.1);
    b = 0;
    wait(1);
}