File content as of revision 0:18550647a842:

#include "mbed.h"
#include "Motor.h"
#include "rtos.h"
#include "stdio.h"
#include "ShiftBrite.h"
#include "Speaker.h"

// I/O Configurations 

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

// Motors
Motor m_l(p21, p22, p23); // pwm, fwd, rev
Motor m_r(p26, p30, p24); // pwm, fwd, rev

//Shiftbrites
SPI spi(p11, p12, p13);
ShiftBrite myBrite(p15,p16,spi); //latch, enable, spi

//Speaker
Speaker mySpeaker(p25);

// Serial ports
RawSerial pc(USBTX, USBRX);
RawSerial dev(p28, p27);
char X;
Mutex dev_mutex;
DigitalOut rst1(p8);

// IR 
AnalogIn IR(p20);
float IR_distance;

// Serial reading variables
char building = '0';
int tVal = 0;
int l_speed = 32767;
int r_speed = 32767;
int c;

// Horn and Light flags
bool horn = false;
int lightMode = 0;
int colorMode = 0;

// Threads

// Motor Thread
// Uses thresholds for the Y-axis of the analog joysticks to set variable motor speed
void thread_Motor(void const *args){
    while (true) {
              
        m_r.speed(0); 
        m_l.speed(0);
        
        // LEFT MOTOR CONTROL
        if(l_speed < 30000){
            if(l_speed < 8000)
                m_l.speed(-1);
            else if(l_speed < 16000)
                m_l.speed(-0.8);
            else if(l_speed < 24000)
                m_l.speed(-0.6);
            else
                m_l.speed(-0.5);
       }
        else if(l_speed > 34000 & IR_distance < 0.9f){
            if(l_speed > 56000)
                m_l.speed(1);
            else if(l_speed > 48000)
                m_l.speed(0.8);
            else if(l_speed > 40000)
                m_l.speed(0.6);
            else
                m_l.speed(0.5);
        } 
        else
            m_l.speed(0);
   
        // RIGHT MOTOR CONTROL
        if(r_speed < 30000){
            if(r_speed < 8000)
                m_r.speed(-1);
            else if(r_speed < 16000)
                m_r.speed(-0.8);
            else if(r_speed < 24000)
                m_r.speed(-0.6);
            else
                m_r.speed(-0.5);
       }
        else if(r_speed > 34000 & IR_distance < 0.9f){
            if(r_speed > 56000)
                m_r.speed(1);
            else if(r_speed > 48000)
                m_r.speed(0.8);
            else if(r_speed > 40000)
                m_r.speed(0.6);
            else
                m_r.speed(0.5);
        } 
        else
            m_r.speed(0);
        Thread::wait(10);
    }
}


// Serial Port Reading Interrupt
void dev_recv(){
    led1 = !led1;
    while(dev.readable()) {
        dev_mutex.lock();
        X = dev.getc();
        dev_mutex.unlock();
        switch (X) {
            case 'L': //number button 1
                building = 'L';
                tVal = 0;
                break;
            case 'R': //number button 2
                building = 'R';
                tVal = 0;
                break;
            case ']': //number button 3
                if(building == 'L')
                    l_speed = tVal;
                else
                    r_speed = tVal;
                building = '0';
                break;
            case 'T': //number button 2
                horn = true;
                break;
            case 't': //number button 2
                horn = false;
                break;
            case 'S': //number button 2
                lightMode++;
                break;
            case 'C': //number button 2
                colorMode++;
                break;
            default:
                break;                   
        }
        c = (int)X - 48;
        if(building != '0' && c >= 0 && c <= 9)
            tVal = tVal*10 + c;
    }
}

// Shiftbrite Thread
void thread_Lights(void const *args){
        int r,g,b;
    r=g=b=0;
    int br = 0;
    int scale = 0;
    //myBrite.Brightness(1023,1023,1023);
    //myBrite.Write(0,0,0);
    while(1) {
        int mode = lightMode%3; 
        int color = colorMode % 4;
        switch (color){
            case 0: 
                r=g=b=1;
                break;
            case 1: 
                r = 1;
                g=b=0;
                break;
            case 2: 
                g = 1;
                r=b=0;
                break;
            case 3: 
                b = 1;
                r=g=0;
                break;
            default:
                break;   
        }     
        
        //pc.printf("%d,%d,%d\r\n",r,g,b);
        switch (mode){
            case 0:
                scale = 0;
                br = 0;
                break;
            case 1:
                scale = 100;
                br = 600;
                break;
            case 2:
                scale= 255;
                br = 1023;
                break;
            default:
                break;
        }
        r = (int) (r * scale);
        b = (int) (b * scale);
        g = (int) (g * scale);
        myBrite.Write(r,g,b);
        myBrite.Brightness(br*r,br*g,br*b);
        
        Thread::wait(50);
    }
}

// Speaker Thread
void thread_Speaker(void const *args){
    while(1){
        if (horn){
            mySpeaker.PlayNote(400.0,0.2,0.5);    
        }
    Thread::wait(10);
    }
}

// Main
int main(){
    pc.baud(9600);

    dev.attach(&dev_recv, Serial::RxIrq);
    
    Thread t2(thread_Motor);
    Thread t3(thread_Lights);
    Thread t4(thread_Speaker);
    
    while(1) {
        if(IR > 0.9f){
            led1 = 1;
            led2 = 1;
            led3 = 1;
            led4 = 1;
        }
        else if(IR > 0.7f){
            led1 = 1;
            led2 = 1;
            led3 = 1;
            led4 = 0;   
        }
        else if(IR > 0.5f){
            led1 = 1;
            led2 = 1;
            led3 = 0;
            led4 = 0;   
        }
        else if(IR > 0.3f){
            led1 = 1;
            led2 = 0;
            led3 = 0;
            led4 = 0;   
        }
        else if(IR > 0.1f){
            led1 = 0;
            led2 = 0;
            led3 = 0;
            led4 = 0;
        }
        IR_distance = IR;
        Thread::wait(50);   // wait 0.5s
    }
}