James Plager / Mbed 2 deprecated Roomba_Project

Robot that currently does nothing

Dependencies:   4DGL-uLCD-SE SDFileSystem ShiftBrite mbed-rtos mbed wave_player Motor

main.cpp@11:9d9ab1dfa28a, 2017-03-16 (annotated)

Committer:
CRaslawski
Date:
Thu Mar 16 16:35:43 2017 +0000
Revision:
11:9d9ab1dfa28a
Parent:
10:059154e51621 
Added bluetooth stubs

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jplager3 0:ada50658d850 1 #include "mbed.h"
jplager3 0:ada50658d850 2 #include "rtos.h"
jplager3 0:ada50658d850 3 #include "uLCD_4DGL.h"
jplager3 0:ada50658d850 4 //#include "ShiftBrite.h"
jplager3 0:ada50658d850 5 #include "SDFileSystem.h"
jplager3 0:ada50658d850 6 #include "wave_player.h"
jplager3 8:cde09ae38be7 7 #include "Motor.h"
jplager3 0:ada50658d850 8
jplager3 10:059154e51621 9 Mutex mutex; // make sure only one thread trying to write to uLCD at a time
jplager3 0:ada50658d850 10 Mutex mutex2; //
jplager3 0:ada50658d850 11 DigitalOut led1(LED1);
jplager3 0:ada50658d850 12 DigitalOut led2(LED2);
jplager3 0:ada50658d850 13 DigitalOut led3(LED3);
jplager3 0:ada50658d850 14 DigitalOut led4(LED4);
jplager3 0:ada50658d850 15 Thread thread1;
jplager3 0:ada50658d850 16 Thread thread2;
jplager3 0:ada50658d850 17 Thread thread3;
jplager3 0:ada50658d850 18 Thread thread4;
jplager3 0:ada50658d850 19 SPI spi(p11, p12, p13);
jplager3 0:ada50658d850 20 uLCD_4DGL uLCD(p28, p27, p30);
jplager3 0:ada50658d850 21 SDFileSystem sd(p5, p6, p7, p8, "sd"); //SD card
jplager3 0:ada50658d850 22 AnalogOut DACout(p18); //must be p18
CRaslawski 5:6ca141fc6c4e 23 AnalogIn IR(p20);
jplager3 0:ada50658d850 24 FILE *wave_file = NULL; //global bc its gotta be changed by Main while running in child thread
CRaslawski 6:21c5a7cf63de 25 FILE *wave_file2 = NULL;
jplager3 0:ada50658d850 26 wave_player waver(&DACout); //create wave_player object for speaker
jplager3 8:cde09ae38be7 27 //create objects for Motor Control
jplager3 8:cde09ae38be7 28 //PwmOut motorL(p21); // doesnt seem to work
jplager3 8:cde09ae38be7 29 //Motor Left(p21, p12, p11); //, p6, p5, 1); // pwm, fwd, rev, can brake
jplager3 8:cde09ae38be7 30 Motor Left(p21, p14, p13); // green wires. pwm, fwd, rev, add ", 1" for braking
jplager3 8:cde09ae38be7 31 Motor Right(p22, p12, p11); // red wires
CRaslawski 11:9d9ab1dfa28a 32
CRaslawski 11:9d9ab1dfa28a 33 /*
CRaslawski 11:9d9ab1dfa28a 34 //Bluetooth addition
CRaslawski 11:9d9ab1dfa28a 35 RawSerial pc(USBTX, USBRX);
CRaslawski 11:9d9ab1dfa28a 36 RawSerial dev(p9,p10); //tx, rx
CRaslawski 11:9d9ab1dfa28a 37
CRaslawski 11:9d9ab1dfa28a 38 void dev_recv()
CRaslawski 11:9d9ab1dfa28a 39 {
CRaslawski 11:9d9ab1dfa28a 40 //led1 = !led1;
CRaslawski 11:9d9ab1dfa28a 41 while(dev.readable()) {
CRaslawski 11:9d9ab1dfa28a 42 pc.putc(dev.getc());
CRaslawski 11:9d9ab1dfa28a 43 }
CRaslawski 11:9d9ab1dfa28a 44 }
CRaslawski 11:9d9ab1dfa28a 45
CRaslawski 11:9d9ab1dfa28a 46 void pc_recv()
CRaslawski 11:9d9ab1dfa28a 47 {
CRaslawski 11:9d9ab1dfa28a 48 led4 = !led4;
CRaslawski 11:9d9ab1dfa28a 49 while(pc.readable()) {
CRaslawski 11:9d9ab1dfa28a 50 dev.putc(pc.getc());
CRaslawski 11:9d9ab1dfa28a 51 }
CRaslawski 11:9d9ab1dfa28a 52 }
CRaslawski 11:9d9ab1dfa28a 53 */
CRaslawski 11:9d9ab1dfa28a 54
jplager3 0:ada50658d850 55 void LCD_thread1() {
jplager3 0:ada50658d850 56 while(1){
jplager3 0:ada50658d850 57 mutex.lock();
jplager3 0:ada50658d850 58 uLCD.filled_circle(64, 64, 12, 0xFF0000);
jplager3 0:ada50658d850 59 mutex.unlock();
jplager3 0:ada50658d850 60 wait(.5);
jplager3 0:ada50658d850 61 mutex.lock();
jplager3 0:ada50658d850 62 uLCD.filled_circle(64, 64, 12, 0x0000FF);
jplager3 0:ada50658d850 63 mutex.unlock();
jplager3 0:ada50658d850 64 wait(.5);
jplager3 0:ada50658d850 65 }
jplager3 10:059154e51621 66 }
jplager3 10:059154e51621 67 //LCD fucntions to display status
jplager3 10:059154e51621 68 void LCD_GRN(){
jplager3 10:059154e51621 69 //mutex.lock();
jplager3 10:059154e51621 70 uLCD.cls(); // clear scrn
jplager3 10:059154e51621 71 uLCD.background_color(GREEN);
jplager3 10:059154e51621 72 uLCD.locate(7,7); // place cursor middle of screen
jplager3 10:059154e51621 73 uLCD.printf("GOOD");
jplager3 10:059154e51621 74
jplager3 10:059154e51621 75 //mutex.unlock(); //free mutex
jplager3 0:ada50658d850 76 }
jplager3 10:059154e51621 77 void LCD_YEL(){
jplager3 10:059154e51621 78 //mutex.lock();
jplager3 10:059154e51621 79 uLCD.cls();
jplager3 10:059154e51621 80 uLCD.background_color(YELLOW);
jplager3 10:059154e51621 81 uLCD.locate(4, 8); // place cursor middle of screen
jplager3 10:059154e51621 82 uLCD.printf("Collision");
jplager3 10:059154e51621 83 uLCD.locate(4,9);
jplager3 10:059154e51621 84 uLCD.printf("Imminent");
jplager3 10:059154e51621 85 //mutex.unlock();
jplager3 10:059154e51621 86 }
jplager3 10:059154e51621 87 void LCD_RED(){
jplager3 10:059154e51621 88 //mutex.lock();
jplager3 10:059154e51621 89 uLCD.cls();
jplager3 10:059154e51621 90 uLCD.background_color(RED);
jplager3 10:059154e51621 91 uLCD.textbackground_color(BLACK);
jplager3 10:059154e51621 92 uLCD.locate(2, 8); // place cursor middle of screen
jplager3 10:059154e51621 93 uLCD.printf("REVERSING");
jplager3 10:059154e51621 94 //mutex.unlock();
jplager3 10:059154e51621 95 }
jplager3 0:ada50658d850 96 void sound_thread(){
CRaslawski 4:589e4a2028a7 97 while(1) {
CRaslawski 4:589e4a2028a7 98 //FILE *wave_file;
CRaslawski 4:589e4a2028a7 99 wave_file=fopen("/sd/vacuum.wav","r");
CRaslawski 4:589e4a2028a7 100 if (wave_file == NULL){
CRaslawski 6:21c5a7cf63de 101 led1=led4 = 1; // if file read error, outside LEDs are on
CRaslawski 6:21c5a7cf63de 102 led2=led3 = 0;
CRaslawski 4:589e4a2028a7 103 }
CRaslawski 4:589e4a2028a7 104 waver.play(wave_file);
CRaslawski 4:589e4a2028a7 105 fclose(wave_file);
jplager3 0:ada50658d850 106 }
jplager3 0:ada50658d850 107 }
CRaslawski 5:6ca141fc6c4e 108
CRaslawski 6:21c5a7cf63de 109 void beep(){
CRaslawski 6:21c5a7cf63de 110 wave_file2=fopen("/sd/beep.wav","r");
CRaslawski 6:21c5a7cf63de 111 if(wave_file2 == NULL) {
CRaslawski 6:21c5a7cf63de 112 led1=led4 = 0; // if file read error, inside LEDs are on
CRaslawski 6:21c5a7cf63de 113 led2=led3 = 0;
CRaslawski 6:21c5a7cf63de 114 }
CRaslawski 6:21c5a7cf63de 115 waver.play(wave_file2);
CRaslawski 6:21c5a7cf63de 116 fclose(wave_file2);
CRaslawski 6:21c5a7cf63de 117 }
jplager3 9:37bb5f0e2975 118 void drive(float speed){
jplager3 9:37bb5f0e2975 119 Left.speed(speed);
jplager3 9:37bb5f0e2975 120 Right.speed(speed);
jplager3 9:37bb5f0e2975 121 }
jplager3 9:37bb5f0e2975 122 void reverse(float speed){
jplager3 9:37bb5f0e2975 123 Left.speed(-speed);
jplager3 9:37bb5f0e2975 124 Right.speed(-speed);
jplager3 9:37bb5f0e2975 125 }
jplager3 9:37bb5f0e2975 126 void turnRight(float speed){
jplager3 9:37bb5f0e2975 127 Left.speed(speed);
jplager3 9:37bb5f0e2975 128 Right.speed(-speed);
jplager3 9:37bb5f0e2975 129 //wait(0.7);
jplager3 9:37bb5f0e2975 130 }
jplager3 9:37bb5f0e2975 131 void turnLeft(float speed){
jplager3 9:37bb5f0e2975 132 Left.speed(-speed);
jplager3 9:37bb5f0e2975 133 Right.speed(speed);
jplager3 9:37bb5f0e2975 134 //wait(0.7);
jplager3 9:37bb5f0e2975 135 }
jplager3 9:37bb5f0e2975 136 void stop(){
jplager3 9:37bb5f0e2975 137 Left.speed(0.0);
jplager3 9:37bb5f0e2975 138 Right.speed(0.0);
jplager3 9:37bb5f0e2975 139 }
jplager3 9:37bb5f0e2975 140
jplager3 0:ada50658d850 141 int main() {
CRaslawski 11:9d9ab1dfa28a 142
CRaslawski 11:9d9ab1dfa28a 143 /*
CRaslawski 11:9d9ab1dfa28a 144 //BT stuff
CRaslawski 11:9d9ab1dfa28a 145 pc.baud(9600);
CRaslawski 11:9d9ab1dfa28a 146 dev.baud(9600);
CRaslawski 11:9d9ab1dfa28a 147 pc.attach(&pc_recv, Serial::RxIrq);
CRaslawski 11:9d9ab1dfa28a 148 dev.attach(&dev_recv, Serial::RxIrq);
CRaslawski 11:9d9ab1dfa28a 149 */
CRaslawski 11:9d9ab1dfa28a 150
CRaslawski 11:9d9ab1dfa28a 151
jplager3 9:37bb5f0e2975 152 Left.speed(0.0); //attempt at forcing Left to chill on startup. ineffective.
jplager3 9:37bb5f0e2975 153 Right.speed(0.0); //Right does not have the issue. Is chill on startup
jplager3 0:ada50658d850 154 //thread1.start(IR_thread); // read in IR data
jplager3 10:059154e51621 155 //thread2.start(LCD_thread1);
jplager3 0:ada50658d850 156 //thread3.start(Motor_thread);
jplager3 0:ada50658d850 157 thread4.start(sound_thread);
jplager3 0:ada50658d850 158
jplager3 9:37bb5f0e2975 159 float turnDirectionSelector = 0.3;
jplager3 9:37bb5f0e2975 160 drive(0.3); // begin driving fwd
jplager3 10:059154e51621 161 //thread1.start(LCD_GRN); // I think when thread closes, LCD reverts to black.
jplager3 10:059154e51621 162 LCD_GRN();
jplager3 10:059154e51621 163 //bool screenNotWritten = false;
jplager3 9:37bb5f0e2975 164 while(1){
CRaslawski 5:6ca141fc6c4e 165 //IR testing
jplager3 10:059154e51621 166 if(IR>0.3 && IR<0.5) {
jplager3 10:059154e51621 167 LCD_GRN(); // scan for object
CRaslawski 5:6ca141fc6c4e 168 led1=1;
CRaslawski 5:6ca141fc6c4e 169 led2=led3=led4=0;
CRaslawski 5:6ca141fc6c4e 170 } else if (IR>0.5 && IR< 0.7) {
jplager3 10:059154e51621 171 //thread2.start(LCD_YEL);
jplager3 10:059154e51621 172 LCD_GRN();
CRaslawski 5:6ca141fc6c4e 173 led1=led2=1;
CRaslawski 5:6ca141fc6c4e 174 led3=led4=0;
CRaslawski 5:6ca141fc6c4e 175 } else if (IR>0.7 && IR<0.9) {
jplager3 10:059154e51621 176 LCD_YEL();
CRaslawski 5:6ca141fc6c4e 177 led1=led2=led3=1;
CRaslawski 5:6ca141fc6c4e 178 led4=0;
CRaslawski 6:21c5a7cf63de 179 } else if(IR>0.9){ //collision threshold seems to be the same from IR=0.8 to IR=0.95
jplager3 9:37bb5f0e2975 180 // in this block, "collision" detected. Back up, execute turn, and proceed driving fwd.
jplager3 10:059154e51621 181 //thread3.start(LCD_RED);
jplager3 10:059154e51621 182
jplager3 10:059154e51621 183 //LCD_RED();
CRaslawski 6:21c5a7cf63de 184 led1=led2=led3=led4=1;
CRaslawski 6:21c5a7cf63de 185 fclose(wave_file);
CRaslawski 6:21c5a7cf63de 186 thread4.terminate();
CRaslawski 6:21c5a7cf63de 187 thread4.start(beep);
jplager3 9:37bb5f0e2975 188 // reverse motors and turn Roomba here
jplager3 9:37bb5f0e2975 189 stop();
jplager3 9:37bb5f0e2975 190 wait(0.4);
jplager3 9:37bb5f0e2975 191 reverse(0.3);
jplager3 9:37bb5f0e2975 192 wait(2);
jplager3 9:37bb5f0e2975 193 turnDirectionSelector*=-1; // will alternate turning left/right upon collision
jplager3 9:37bb5f0e2975 194 turnRight(turnDirectionSelector);
jplager3 9:37bb5f0e2975 195 wait(1); //2s is a bit more than 180deg @ 0.3 speed
jplager3 9:37bb5f0e2975 196 stop();
jplager3 9:37bb5f0e2975 197
jplager3 10:059154e51621 198 wait(0.7); //wait for beeping to finish
jplager3 10:059154e51621 199
CRaslawski 6:21c5a7cf63de 200 fclose(wave_file2);
CRaslawski 6:21c5a7cf63de 201 thread4.terminate();
CRaslawski 6:21c5a7cf63de 202 thread4.start(sound_thread);
jplager3 9:37bb5f0e2975 203 drive(0.4);
jplager3 10:059154e51621 204 //thread1.start(LCD_GRN);
CRaslawski 5:6ca141fc6c4e 205 } else led1=led2=led3=led4=0;
jplager3 0:ada50658d850 206 }
jplager3 0:ada50658d850 207 }