File content as of revision 4:8588557fbd75:

//**************************
// Mbed to Raspberry Pi
//**************************
// 0 - pass starting line
// 1 - pass checkpoint 1
// 2 - pass checkpoint 2
// ...
// 9 - end race

/*

    Color Coding

Finish Line:
    red     +
    black   -
    yellow  mbed out (rbg ctrl)
    red     rbgRed
    blue    pwm (audio)
    white   echo
    orange  trigger

Checkpoint 1-2
    red     +
    black   -
    white   echo
    orange  trigger
    yellow  led ctrl

*/

// Includes
#include "mbed.h"
#include "ultrasonic.h"
#include "rtos.h"
#include "SevenSegmentSerial.h"

// Threads
Thread      thread1;
Thread      thread2;
// PC Serial
Serial      pc(USBTX,USBRX);
//LED Ctrls (1 per CP)
DigitalOut  led0(p5);
DigitalOut  led1(p6);
DigitalOut  led2(p7);
// Finish Line RGB Ctrl
DigitalOut  rgbCtrl1(p8);
DigitalOut  rgbCtrl2(p9);
DigitalOut  rgbCtrl3(p10);
// Finish Line RGB Colors
DigitalOut  rgbRed(p23);
DigitalOut  rgbGreen(p24);
PwmOut      speaker(p25);
// Seven Segment Display
SevenSegmentSerial ssd(UART_MODE, p28);

// Globals
int width[3];          //width of each checkpoint, mm
int sensitivity[3];   //sensitivity, adjust with testing, mm
int total_laps = 3;
int current_lap = 0;
int B4Note = 500;
int E5Note = 659;
int i = 500;
bool early_start = 0;
bool start_done = 0;
int setup[3] = {1,1,1};

//state machine, tracks current position in track
enum state_type {start,past_start,past_cp1,past_cp2};
state_type track_state = start;

// Race start lights and sound
void start_race()
{
    // 3
    ssd.setDigit(3,0);
    speaker.period(1.0/B4Note);
    speaker =0.5;
    rgbRed = 1;
    rgbCtrl1 = 1;
    Thread::wait(500);
    speaker = 0;
    Thread::wait(500);

    // 2
    ssd.setDigit(2,1);
    ssd = 2;
    speaker =0.5;
    rgbCtrl2 = 1;
    Thread::wait(500);
    speaker = 0;
    Thread::wait(500);

    // 1
    ssd.setDigit(1,2);
    speaker =0.5;
    rgbCtrl3 = 1;
    Thread::wait(500);
    speaker = 0;
    Thread::wait(500);

    // GO!
    ssd.write("GOOO");
    rgbRed = 0;
    rgbGreen = 1;
    start_done = 1;
    speaker.period(1.0/E5Note);
    speaker =0.5;
    Thread::wait(1000);
    speaker = 0;
    rgbCtrl1 = 0;
    rgbCtrl2 = 0;
    rgbCtrl3 = 0;
}

// Plays bleep for crossing finish line
void play_audio()
{
    speaker.period(1.0/B4Note); // 500hz period
    speaker =0.5; //50% duty cycle - max volume
    Thread::wait(150);
    speaker=0.0; // turn off audio
}

//executed when crossing starting line
void start_line(int distance)
{
    if (setup[0]) {
        pc.printf("Setup\n");
        pc.printf("%d",distance);
        width[0] = distance;
        setup[0] = !setup[0];
        sensitivity[0] = 0.1f*width[0];
    }

    if (width[0] - distance >= sensitivity[0]) {
        pc.printf("Past start line\n");
        led0 = 1;
        led1 = 0;
        led2 = 0;
        current_lap++;
        pc.printf("%d\n",current_lap);
        track_state = past_start;
        pc.putc('0');
        thread1.start(&play_audio);
    }
    
}

//executed when passing checkpoint 1
void checkpoint1(int distance)
{
    if (setup[1]) {
        pc.printf("Setup\n");
        pc.printf("%d",distance);
        width[1] = distance;
        setup[1] = !setup[1];
        sensitivity[1] = 0.1f*width[1];
    }

    pc.printf("Past Checkpoint 1\n");
    if (width[1] - distance >= sensitivity[1]) {
        led1 = 1;
        track_state = past_cp1;
        pc.putc('1');
    }
}

//executed when passing checkpoint 2
void checkpoint2(int distance)
{
    if (setup[2]) {
        pc.printf("Setup\n");
        pc.printf("%d",distance);
        width[2] = distance;
        setup[2] = !setup[2];
        sensitivity[2] = 0.1f*width[2];
    }

    pc.printf("Past Checkpoint 2\n");
    if (width[2] - distance >= sensitivity[2]) {
        led2 = 1;
        track_state = past_cp2;
        pc.putc('2');
    }
}
// Sonars (19-20 CANNOT be set for sonar, they don't allow for interrupts)
ultrasonic  sonar0(p15,p16,0.1,0.1,&start_line);      //starting line
ultrasonic  sonar1(p17,p18,0.1,0.1,&checkpoint1);     //checkpoint 1
ultrasonic  sonar2(p21,p22,0.1,0.1,&checkpoint2);     //checkpoint 2

int main()
{
    // Reset
    ssd.clear();

    // Settings
    ssd.setBrightness(100);

    // Start Sonars
    sonar0.startUpdates();
    sonar1.startUpdates();
    sonar2.startUpdates();
    sonar0.checkDistance();
    sonar1.checkDistance();
    sonar2.checkDistance();

    // Start Race Lights + Sound
    thread2.start(&start_race);
    while(!start_done) {
        if (current_lap) {
            // Flash everything red. Play bad sounds.
        }
        Thread::wait(50);
    }

    // Race Checkpoint Tracking
    while(1) {
        switch (track_state) {
            case start:
                sonar0.checkDistance();
                break;
            case past_start:
                sonar1.checkDistance();
                break;
            case past_cp1:
                sonar2.checkDistance();
                break;
            case past_cp2:
                sonar0.checkDistance();
                break;
        }
        if (current_lap > total_laps) {
            pc.putc('9');
            speaker.period(1.0/E5Note); // 500hz period
            speaker =0.5; //50% duty cycle - max volume
            Thread::wait(1000);
            speaker=0.0; // turn off audio
            while(1) {
                pc.printf("Race Over");
                Thread::wait(50);
            }
        }
        Thread::wait(50);
    }
}