main.cpp

00001 #include "mbed.h"
00002 #include "rtos.h"
00003 #include "uLCD_4DGL.h"
00004 //#include "ShiftBrite.h"
00005 #include "SDFileSystem.h"
00006 #include "wave_player.h"
00007 #include "Motor.h"
00008 
00009 Mutex mutex;    // make sure only one thread trying to write to uLCD at a time
00010 Mutex mutex2;   //
00011 DigitalOut led1(LED1);
00012 DigitalOut led2(LED2);
00013 DigitalOut led3(LED3);
00014 DigitalOut led4(LED4);
00015 Thread thread1;
00016 Thread thread2;
00017 Thread thread3;
00018 Thread thread4;
00019 SPI spi(p11, p12, p13);
00020 uLCD_4DGL uLCD(p28, p27, p30); 
00021 SDFileSystem sd(p5, p6, p7, p8, "sd"); //SD card
00022 AnalogOut DACout(p18);      //must be p18
00023 AnalogIn IR(p20);
00024 FILE *wave_file = NULL;         //global bc its gotta be changed by Main while running in child thread
00025 FILE *wave_file2 = NULL;
00026 wave_player waver(&DACout);     //create wave_player object for speaker
00027 //create objects for Motor Control
00028 //PwmOut motorL(p21);           // doesnt seem to work
00029 //Motor Left(p21, p12, p11); //, p6, p5, 1); // pwm, fwd, rev, can brake
00030 Motor Left(p21, p14, p13);      // green wires. pwm, fwd, rev, add ", 1" for braking
00031 Motor Right(p22, p12, p11);     // red wires
00032 
00033 /*
00034 //Bluetooth addition
00035 RawSerial  pc(USBTX, USBRX);
00036 RawSerial  dev(p9,p10); //tx, rx
00037 
00038 void dev_recv()
00039 {
00040     //led1 = !led1;
00041     while(dev.readable()) {
00042         pc.putc(dev.getc());
00043     }
00044 }
00045 
00046 void pc_recv()
00047 {
00048     led4 = !led4;
00049     while(pc.readable()) {
00050         dev.putc(pc.getc());
00051     }
00052 }
00053 */
00054 
00055 void LCD_thread1() {
00056     while(1){    
00057         mutex.lock();
00058         uLCD.filled_circle(64, 64, 12, 0xFF0000);
00059         mutex.unlock();
00060         wait(.5);
00061         mutex.lock();
00062         uLCD.filled_circle(64, 64, 12, 0x0000FF);
00063         mutex.unlock();
00064         wait(.5);
00065     }     
00066 }
00067 //LCD fucntions to display status
00068 void LCD_GRN(){
00069     //mutex.lock();
00070     uLCD.cls();         // clear scrn
00071     uLCD.background_color(GREEN);     
00072     uLCD.locate(7,7);        // place cursor middle of screen
00073     uLCD.printf("GOOD");
00074     
00075     //mutex.unlock();             //free mutex
00076 } 
00077 void LCD_YEL(){
00078     //mutex.lock();
00079     uLCD.cls();
00080     uLCD.background_color(YELLOW);     
00081     uLCD.locate(4, 8);        // place cursor middle of screen
00082     uLCD.printf("Collision");
00083     uLCD.locate(4,9);
00084     uLCD.printf("Imminent");
00085     //mutex.unlock();
00086 }
00087 void LCD_RED(){
00088     //mutex.lock();
00089     uLCD.cls();
00090     uLCD.background_color(RED);   
00091     uLCD.textbackground_color(BLACK);  
00092     uLCD.locate(2, 8);        // place cursor middle of screen
00093     uLCD.printf("REVERSING");    
00094     //mutex.unlock();
00095 }
00096 void sound_thread(){
00097     while(1) {
00098         //FILE *wave_file;
00099         wave_file=fopen("/sd/vacuum.wav","r");    
00100         if (wave_file == NULL){
00101                led1=led4 = 1; // if file read error, outside LEDs are on
00102                led2=led3 = 0;
00103         }
00104         waver.play(wave_file);
00105         fclose(wave_file);  
00106     }
00107 }
00108 
00109 void beep(){
00110     wave_file2=fopen("/sd/beep.wav","r");
00111     if(wave_file2 == NULL) {
00112         led1=led4 = 0; // if file read error, inside LEDs are on
00113         led2=led3 = 0;
00114     }
00115     waver.play(wave_file2);
00116     fclose(wave_file2);  
00117 }
00118 void drive(float speed){
00119     Left.speed(speed);
00120     Right.speed(speed);    
00121 }
00122 void reverse(float speed){
00123     Left.speed(-speed);
00124     Right.speed(-speed);
00125 }
00126 void turnRight(float speed){
00127     Left.speed(speed);
00128     Right.speed(-speed);
00129     //wait(0.7);
00130 }
00131 void turnLeft(float speed){
00132     Left.speed(-speed);
00133     Right.speed(speed);
00134     //wait(0.7);
00135 }
00136 void stop(){
00137     Left.speed(0.0);
00138     Right.speed(0.0);
00139 }
00140 
00141 int main() {
00142     
00143     /*
00144     //BT stuff
00145     pc.baud(9600);
00146     dev.baud(9600);
00147     pc.attach(&pc_recv, Serial::RxIrq);
00148     dev.attach(&dev_recv, Serial::RxIrq);
00149     */
00150     
00151     
00152     Left.speed(0.0);        //attempt at forcing Left to chill on startup. ineffective.
00153     Right.speed(0.0);       //Right does not have the issue. Is chill on startup
00154     //thread1.start(IR_thread); // read in IR data
00155     //thread2.start(LCD_thread1);   
00156     //thread3.start(Motor_thread);
00157     thread4.start(sound_thread);
00158     
00159     float turnDirectionSelector = 0.3;
00160     drive(0.3);    // begin driving fwd    
00161     //thread1.start(LCD_GRN);           // I think when thread closes, LCD reverts to black.
00162     LCD_GRN();
00163     //bool screenNotWritten = false;
00164     while(1){        
00165         //IR testing
00166         if(IR>0.3 && IR<0.5) {
00167             LCD_GRN();              // scan for object
00168             led1=1;
00169             led2=led3=led4=0;
00170         } else if (IR>0.5 && IR< 0.7) {
00171             //thread2.start(LCD_YEL);
00172             LCD_GRN();
00173             led1=led2=1;
00174             led3=led4=0;
00175         } else if (IR>0.7 && IR<0.9) {
00176             LCD_YEL();
00177             led1=led2=led3=1;
00178             led4=0;
00179         } else if(IR>0.9){ //collision threshold seems to be the same from IR=0.8 to IR=0.95
00180                             // in this block, "collision" detected. Back up, execute turn, and proceed driving fwd.
00181             //thread3.start(LCD_RED);
00182             
00183             //LCD_RED();
00184             led1=led2=led3=led4=1; 
00185             fclose(wave_file);
00186             thread4.terminate();
00187             thread4.start(beep);
00188             // reverse motors and turn Roomba here
00189             stop();
00190             wait(0.4);
00191             reverse(0.3);
00192             wait(2);
00193             turnDirectionSelector*=-1;      // will alternate turning left/right upon collision
00194             turnRight(turnDirectionSelector);
00195             wait(1);    //2s is a bit more than 180deg @ 0.3 speed
00196             stop();
00197             
00198             wait(0.7); //wait for beeping to finish
00199             
00200             fclose(wave_file2);
00201             thread4.terminate();
00202             thread4.start(sound_thread);
00203             drive(0.4);
00204             //thread1.start(LCD_GRN);
00205         } else led1=led2=led3=led4=0;
00206     }
00207 }