File content as of revision 0:d9d5943dfa45:

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

DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);

Serial pc(USBTX, USBRX); // tx, rx

DigitalIn safe(p9);

Serial blue(p28,p27);

Motor motor_left(p23, p6, p5);
Motor motor_right(p24, p8, p7);

DigitalOut trigger(p13);
DigitalIn  echo(p14);

Speaker mySpeaker(p18);

Servo servo(p26);

Timer sonar;

void chooseLed(int num) {
    if (num == 1) {
        led1 = 1;
        led2 = 0;
        led3 = 0;
        led4 = 0;
    } else if (num == 2) {
        led1 = 0;
        led2 = 1;
        led3 = 0;
        led4 = 0;
    } else if (num == 3) {
        led1 = 0;
        led2 = 0;
        led3 = 1;
        led4 = 0;
    } else if (num == 4) {
        led1 = 0;
        led2 = 0;
        led3 = 0;
        led4 = 1;
    }
}


int getSonar(int correction) {
        int distance = 0;
        trigger = 1;
        sonar.reset();
        wait_us(10.0);
        trigger = 0;

        while (echo==0) {};
        sonar.start();
        while (echo==1) {};

        sonar.stop();

        distance = (sonar.read_us() - correction) / 58.0;
        return distance;
}

int main() {
    bool tooClose = false;
    sonar.reset();
    sonar.start();

    while (echo==2) {};
    sonar.stop();
    int correction = sonar.read_us();
    int distance = -1;

    char bnum=0;
    char bhit=0;
    
    servo = 0.5;
    
    while(1) {
        if (blue.getc()=='!') {
            if (blue.getc()=='B') { //button data packet
                bnum = blue.getc(); //button number
                bhit = blue.getc(); //1=hit, 0=release
                if ((bnum>='1')&&(bnum<='8')) { //checksum OK?
                    chooseLed(2);
                    pc.printf("%d",0);
                    distance = getSonar(correction); // read sonar data
//                    wait(0.1); // wait echo signal to disperse

                    // check if the distance is too close, set the flag 'tooClose'
                    if (distance > 20) {
                        led3 = 0;
                        led4 = 1;
                        tooClose = false;    
                    } else {
                        led3 = 1;    
                        led4 = 0;
                        tooClose = true;
                    }

                    switch (bnum) {
                        case '1': //button 1 - clockwise servo
                            chooseLed(3);
                            pc.printf("%d",6);
                            if (bhit=='1') {
                                servo = 1.0;
                            } else {
                                servo = 0.5;
                            }
                            break;
                            
                        case '2': //button 2 - counter clockwise servo
                            chooseLed(4);
                            pc.printf("%d",7);
                            if (bhit=='1') {
                                servo = 0;
                            } else {
                                servo = 0.5;
                            }
                            break;
                            
                        case '5': //button 5 up arrow - move forward
                            chooseLed(1);
                            pc.printf("%d",1);
                            if (bhit=='1' && tooClose == 0) {
                                motor_left.speed(1); 
                                motor_right.speed(1);
                                mySpeaker.PlayNote(440, 1.0, 0.2);
                            } else if (tooClose == 1) {
                                motor_left.speed(0);
                                motor_right.speed(0);
                                mySpeaker.PlayNote(880, 0.1, 1.0);
                            } else {
                                motor_left.speed(0);
                                motor_right.speed(0);
                            }
                            break;
                        case '6': //button 6 down arrow - move backward
                            chooseLed(2);
                            pc.printf("%d",2);
                            if (bhit=='1') {
                                motor_left.speed(-1); 
                                motor_right.speed(-1);
                                mySpeaker.PlayNote(600, 0.5, 0.6);
                            } else {
                                motor_left.speed(0);
                                motor_right.speed(0);
                            }
                            break;
                        case '7': //button 7 left arrow - rotate left
                            chooseLed(3);
                            pc.printf("%d",3);
                            if (bhit=='1') {
                                motor_left.speed(-1); 
                                motor_right.speed(1);
                            } else {
                                motor_left.speed(0);
                                motor_right.speed(0);
                            }
                            break;
                        case '8': //button 8 right arrow - rotate right
                            chooseLed(4);
                            pc.printf("%d",4);
                            if (bhit=='1') {
                                motor_left.speed(1); 
                                motor_right.speed(-1);
                            } else {
                                motor_left.speed(0);
                                motor_right.speed(0);
                            }
                            break;
                        default:
                            break;
                    }
                }
            }
        }
    }
}