File content as of revision 12:42d63181dbdb:

#include "mbed.h"
#include "Speaker.h"
#include "ultrasonic.h"
#include "Motor.h"
#include "rtos.h"

RawSerial pc(USBTX, USBRX);
RawSerial bluetooth(p28, p27);
Motor leftWheel(p21, p20, p19);
Motor rightWheel(p22, p15, p14);
Speaker speaker(p23);

float speed = .5;

void bluetooth_thread(void const *args) {
    char cmd, bhit, bnum;
    while(true) {
        if (bluetooth.getc() == '!') {
            cmd = bluetooth.getc();
            switch (cmd) {
                case 'B': // button press
                    bnum = bluetooth.getc();
                    bhit = bluetooth.getc();
                    if (bluetooth.getc()==char(~('!' + 'B' + bnum + bhit))) { //checksum               
                        switch (bnum) {
                            case '1':
                                speed = 1;
                                break;
                            case '2':
                                speed = .75;
                                break;     
                            case '3':
                                speed = .5;
                                break;
                            case '4':
                                speed = .25;
                                break;
                            case '5': //up
                                if (bhit == '1') {
                                    leftWheel.speed(speed);
                                    rightWheel.speed(speed);
                                } else {
                                    leftWheel.speed(0);
                                    rightWheel.speed(0);
                                }
                                break;
                            case '6': //down
                                if (bhit == '1') {
                                    leftWheel.speed(-1 * speed);
                                    rightWheel.speed(-1 * speed);
                                } else {
                                    leftWheel.speed(0);
                                    rightWheel.speed(0);
                                }
                                break;
                            case '7': //left
                                if (bhit == '1') {
                                    leftWheel.speed(-.5);
                                    rightWheel.speed(.5);
                                } else {
                                    leftWheel.speed(0);
                                    rightWheel.speed(0);
                                }
                                break;
                            case '8': //right
                                if (bhit == '1') {
                                    leftWheel.speed(.5);
                                    rightWheel.speed(-.5);
                                } else {
                                    leftWheel.speed(0);
                                    rightWheel.speed(0);
                                }
                                break;
                            default:
                                break;
                        }                       
                    }                    
                    break;
                default:
                    break;
            }
        }
    }
}

void dist(int distance) {
    pc.printf("Distance %d mm\r\n", distance);
    if (distance < 50) {
        speaker.PlayNote(150, 0.15, 0.5);
    } else if (distance < 100) {
        speaker.PlayNote(150, 0.25, 0.02);
    } else if (distance < 200) {
        speaker.PlayNote(150, 0.5, 0.02);
    } else if (distance < 300) {
        speaker.PlayNote(150, 0.75, 0.02);
    }
    Thread::wait(50);
}

ultrasonic sonar(p6, p7, .1, 1, &dist);

int main() {
    sonar.startUpdates();
    
    Thread bt_t(bluetooth_thread);
    while(1) {
        sonar.checkDistance();
    }
}