Robot that currently does nothing
Dependencies: 4DGL-uLCD-SE SDFileSystem ShiftBrite mbed-rtos mbed wave_player Motor
main.cpp
- Committer:
- CRaslawski
- Date:
- 2017-03-16
- Revision:
- 11:9d9ab1dfa28a
- Parent:
- 10:059154e51621
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; } }