File content as of revision 11:9d9ab1dfa28a:

#include "mbed.h"
#include "rtos.h"
#include "uLCD_4DGL.h"
//#include "ShiftBrite.h"
#include "SDFileSystem.h"
#include "wave_player.h"
#include "Motor.h"

Mutex mutex;    // make sure only one thread trying to write to uLCD at a time
Mutex mutex2;   //
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
Thread thread1;
Thread thread2;
Thread thread3;
Thread thread4;
SPI spi(p11, p12, p13);
uLCD_4DGL uLCD(p28, p27, p30); 
SDFileSystem sd(p5, p6, p7, p8, "sd"); //SD card
AnalogOut DACout(p18);      //must be p18
AnalogIn IR(p20);
FILE *wave_file = NULL;         //global bc its gotta be changed by Main while running in child thread
FILE *wave_file2 = NULL;
wave_player waver(&DACout);     //create wave_player object for speaker
//create objects for Motor Control
//PwmOut motorL(p21);           // doesnt seem to work
//Motor Left(p21, p12, p11); //, p6, p5, 1); // pwm, fwd, rev, can brake
Motor Left(p21, p14, p13);      // green wires. pwm, fwd, rev, add ", 1" for braking
Motor Right(p22, p12, p11);     // red wires

/*
//Bluetooth addition
RawSerial  pc(USBTX, USBRX);
RawSerial  dev(p9,p10); //tx, rx

void dev_recv()
{
    //led1 = !led1;
    while(dev.readable()) {
        pc.putc(dev.getc());
    }
}

void pc_recv()
{
    led4 = !led4;
    while(pc.readable()) {
        dev.putc(pc.getc());
    }
}
*/

void LCD_thread1() {
    while(1){    
        mutex.lock();
        uLCD.filled_circle(64, 64, 12, 0xFF0000);
        mutex.unlock();
        wait(.5);
        mutex.lock();
        uLCD.filled_circle(64, 64, 12, 0x0000FF);
        mutex.unlock();
        wait(.5);
    }     
}
//LCD fucntions to display status
void LCD_GRN(){
    //mutex.lock();
    uLCD.cls();         // clear scrn
    uLCD.background_color(GREEN);     
    uLCD.locate(7,7);        // place cursor middle of screen
    uLCD.printf("GOOD");
    
    //mutex.unlock();             //free mutex
} 
void LCD_YEL(){
    //mutex.lock();
    uLCD.cls();
    uLCD.background_color(YELLOW);     
    uLCD.locate(4, 8);        // place cursor middle of screen
    uLCD.printf("Collision");
    uLCD.locate(4,9);
    uLCD.printf("Imminent");
    //mutex.unlock();
}
void LCD_RED(){
    //mutex.lock();
    uLCD.cls();
    uLCD.background_color(RED);   
    uLCD.textbackground_color(BLACK);  
    uLCD.locate(2, 8);        // place cursor middle of screen
    uLCD.printf("REVERSING");    
    //mutex.unlock();
}
void sound_thread(){
    while(1) {
        //FILE *wave_file;
        wave_file=fopen("/sd/vacuum.wav","r");    
        if (wave_file == NULL){
               led1=led4 = 1; // if file read error, outside LEDs are on
               led2=led3 = 0;
        }
        waver.play(wave_file);
        fclose(wave_file);  
    }
}

void beep(){
    wave_file2=fopen("/sd/beep.wav","r");
    if(wave_file2 == NULL) {
        led1=led4 = 0; // if file read error, inside LEDs are on
        led2=led3 = 0;
    }
    waver.play(wave_file2);
    fclose(wave_file2);  
}
void drive(float speed){
    Left.speed(speed);
    Right.speed(speed);    
}
void reverse(float speed){
    Left.speed(-speed);
    Right.speed(-speed);
}
void turnRight(float speed){
    Left.speed(speed);
    Right.speed(-speed);
    //wait(0.7);
}
void turnLeft(float speed){
    Left.speed(-speed);
    Right.speed(speed);
    //wait(0.7);
}
void stop(){
    Left.speed(0.0);
    Right.speed(0.0);
}

int main() {
    
    /*
    //BT stuff
    pc.baud(9600);
    dev.baud(9600);
    pc.attach(&pc_recv, Serial::RxIrq);
    dev.attach(&dev_recv, Serial::RxIrq);
    */
    
    
    Left.speed(0.0);        //attempt at forcing Left to chill on startup. ineffective.
    Right.speed(0.0);       //Right does not have the issue. Is chill on startup
    //thread1.start(IR_thread); // read in IR data
    //thread2.start(LCD_thread1);   
    //thread3.start(Motor_thread);
    thread4.start(sound_thread);
    
    float turnDirectionSelector = 0.3;
    drive(0.3);    // begin driving fwd    
    //thread1.start(LCD_GRN);           // I think when thread closes, LCD reverts to black.
    LCD_GRN();
    //bool screenNotWritten = false;
    while(1){        
        //IR testing
        if(IR>0.3 && IR<0.5) {
            LCD_GRN();              // scan for object
            led1=1;
            led2=led3=led4=0;
        } else if (IR>0.5 && IR< 0.7) {
            //thread2.start(LCD_YEL);
            LCD_GRN();
            led1=led2=1;
            led3=led4=0;
        } else if (IR>0.7 && IR<0.9) {
            LCD_YEL();
            led1=led2=led3=1;
            led4=0;
        } else if(IR>0.9){ //collision threshold seems to be the same from IR=0.8 to IR=0.95
                            // in this block, "collision" detected. Back up, execute turn, and proceed driving fwd.
            //thread3.start(LCD_RED);
            
            //LCD_RED();
            led1=led2=led3=led4=1; 
            fclose(wave_file);
            thread4.terminate();
            thread4.start(beep);
            // reverse motors and turn Roomba here
            stop();
            wait(0.4);
            reverse(0.3);
            wait(2);
            turnDirectionSelector*=-1;      // will alternate turning left/right upon collision
            turnRight(turnDirectionSelector);
            wait(1);    //2s is a bit more than 180deg @ 0.3 speed
            stop();
            
            wait(0.7); //wait for beeping to finish
            
            fclose(wave_file2);
            thread4.terminate();
            thread4.start(sound_thread);
            drive(0.4);
            //thread1.start(LCD_GRN);
        } else led1=led2=led3=led4=0;
    }
}