File content as of revision 0:7d7387eefc5a:

#include "mbed.h"
#include "Motor.h"
#include "Servo.h"
#include "ultrasonic.h"

Motor m1(p25, p15, p14); // pwm, fwd, rev
Motor m2(p26, p19, p20 ); // pwm, fwd, rev
volatile bool move = 1; 
Serial bluemod(p9,p10);
Serial pc(USBTX,USBRX);
Servo myservo(p24);
DigitalOut myled1(LED1); 
DigitalOut myled2(LED2); 
DigitalOut myled3(LED3); 
DigitalOut myled4(LED4); 


 
//C union can convert 4 chars to a float - puts them in same location in memory
//trick to pack the 4 bytes from Bluetooth serial port back into a 32-bit float
union f_or_char {
    float f;
    char  c[4];
};

void dist(int distance)
{
    if (distance < 500.0) {
    m1.speed(0.0);
    m2.speed(0.0);
    wait(1);
    myservo = 0.0;
    wait(1);
    m1.speed(0.3);
    m2.speed(-0.3);
    wait(1.5);
    m1.speed(0.0);
    m2.speed(0.0);
    wait(1.5); 
    move = 0;
    myservo = 1.0;
    myled1 = 1;
    myled2 = 1;
    myled3 = 1;
    myled4 = 1; 
    wait(1);
    myservo = 0.0;
    myled1 = 0;
    myled2 = 0;
    myled3 = 0;
    myled4 = 0; 
    wait(1);
    }
}

ultrasonic sonar(p6, p7, .1, 1, &dist);
 
int main()
{
    float s1;
    float s2;
    char bchecksum=0;
    char temp=0;
    union f_or_char x,y,z;
    
    sonar.startUpdates();//start measuring the distance
    
    while(1) {
        bchecksum=0;
        if (bluemod.getc()=='!') {
            if (bluemod.getc()=='A') { //Accelerometer data packet
                for (int i=0; i<4; i++) {
                    temp = bluemod.getc();
                    x.c[i] = temp;
                    bchecksum = bchecksum + temp;
                }
                for (int i=0; i<4; i++) {
                    temp = bluemod.getc();
                    y.c[i] = temp;
                    bchecksum = bchecksum + temp;
                }
                for (int i=0; i<4; i++) {
                    temp = bluemod.getc();
                    z.c[i] = temp;
                    bchecksum = bchecksum + temp;
                }
                if (bluemod.getc()==char(~('!' + 'A' + bchecksum))) { //checksum OK?
                //pc.printf("X = %f  Y = %f  Z = %f\n\r",x.f, y.f, z.f);         
                    move = 1;
                    if (x.f < -4.0) {
                        s1 = 0.3;
                        s2 = -0.3;
                        m1.speed(s1);
                        m2.speed(s2);
                        myled1 = 1;
                        myled2 = 0;
                        myled3 = 0;
                        myled4 = 0; 
                        wait(0.05);
                        }
                    if (x.f > 4.0) {
                        s1 = -0.3;
                        s2 = 0.3;
                        m1.speed(s1);
                        m2.speed(s2);
                        myled1 = 0;
                        myled2 = 1;
                        myled3 = 0;
                        myled4 = 0; 
                        wait(0.05);
                        }
                    if (y.f < -3.5) {
                        s1 = -0.2;
                        s2 = -0.2;
                        m1.speed(s1);
                        m2.speed(s2);
                        myled1 = 0;
                        myled2 = 0;
                        myled3 = 1;
                        myled4 = 0; 
                        wait(0.05);
                        }
                    if (y.f > 3.0) {
                        s1 = 0.2;
                        s2 = 0.2;
                        m1.speed(s1);
                        m2.speed(s2);
                        myled1 = 0;
                        myled2 = 0;
                        myled3 = 0;
                        myled4 = 1; 
                        wait(0.05);
                        }     
                    if (x.f > -4.0 && x.f < 4.0 && y.f > -4.0 && y.f <4.0) {
                        s1 = 0.0;
                        s2 = 0.0;
                        m1.speed(s1);
                        m2.speed(s2);
                        wait(0.05); 
                        }  
                    while(move) {
                        sonar.checkDistance();
                        move = 0;
                    }        
                }
            }
        }
    }
}