Robot that currently does nothing
Dependencies: 4DGL-uLCD-SE SDFileSystem ShiftBrite mbed-rtos mbed wave_player Motor
main.cpp@10:059154e51621, 2017-03-16 (annotated)
- Committer:
- jplager3
- Date:
- Thu Mar 16 06:48:56 2017 +0000
- Revision:
- 10:059154e51621
- Parent:
- 9:37bb5f0e2975
- Child:
- 11:9d9ab1dfa28a
Robot navigates. Added working LCD support for GOOD state and COLLISION IMMINENT state.
Who changed what in which revision?
User | Revision | Line number | New 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 |
jplager3 | 0:ada50658d850 | 32 | void LCD_thread1() { |
jplager3 | 0:ada50658d850 | 33 | while(1){ |
jplager3 | 0:ada50658d850 | 34 | mutex.lock(); |
jplager3 | 0:ada50658d850 | 35 | uLCD.filled_circle(64, 64, 12, 0xFF0000); |
jplager3 | 0:ada50658d850 | 36 | mutex.unlock(); |
jplager3 | 0:ada50658d850 | 37 | wait(.5); |
jplager3 | 0:ada50658d850 | 38 | mutex.lock(); |
jplager3 | 0:ada50658d850 | 39 | uLCD.filled_circle(64, 64, 12, 0x0000FF); |
jplager3 | 0:ada50658d850 | 40 | mutex.unlock(); |
jplager3 | 0:ada50658d850 | 41 | wait(.5); |
jplager3 | 0:ada50658d850 | 42 | } |
jplager3 | 10:059154e51621 | 43 | } |
jplager3 | 10:059154e51621 | 44 | //LCD fucntions to display status |
jplager3 | 10:059154e51621 | 45 | void LCD_GRN(){ |
jplager3 | 10:059154e51621 | 46 | //mutex.lock(); |
jplager3 | 10:059154e51621 | 47 | uLCD.cls(); // clear scrn |
jplager3 | 10:059154e51621 | 48 | uLCD.background_color(GREEN); |
jplager3 | 10:059154e51621 | 49 | uLCD.locate(7,7); // place cursor middle of screen |
jplager3 | 10:059154e51621 | 50 | uLCD.printf("GOOD"); |
jplager3 | 10:059154e51621 | 51 | |
jplager3 | 10:059154e51621 | 52 | //mutex.unlock(); //free mutex |
jplager3 | 0:ada50658d850 | 53 | } |
jplager3 | 10:059154e51621 | 54 | void LCD_YEL(){ |
jplager3 | 10:059154e51621 | 55 | //mutex.lock(); |
jplager3 | 10:059154e51621 | 56 | uLCD.cls(); |
jplager3 | 10:059154e51621 | 57 | uLCD.background_color(YELLOW); |
jplager3 | 10:059154e51621 | 58 | uLCD.locate(4, 8); // place cursor middle of screen |
jplager3 | 10:059154e51621 | 59 | uLCD.printf("Collision"); |
jplager3 | 10:059154e51621 | 60 | uLCD.locate(4,9); |
jplager3 | 10:059154e51621 | 61 | uLCD.printf("Imminent"); |
jplager3 | 10:059154e51621 | 62 | //mutex.unlock(); |
jplager3 | 10:059154e51621 | 63 | } |
jplager3 | 10:059154e51621 | 64 | void LCD_RED(){ |
jplager3 | 10:059154e51621 | 65 | //mutex.lock(); |
jplager3 | 10:059154e51621 | 66 | uLCD.cls(); |
jplager3 | 10:059154e51621 | 67 | uLCD.background_color(RED); |
jplager3 | 10:059154e51621 | 68 | uLCD.textbackground_color(BLACK); |
jplager3 | 10:059154e51621 | 69 | uLCD.locate(2, 8); // place cursor middle of screen |
jplager3 | 10:059154e51621 | 70 | uLCD.printf("REVERSING"); |
jplager3 | 10:059154e51621 | 71 | //mutex.unlock(); |
jplager3 | 10:059154e51621 | 72 | } |
jplager3 | 0:ada50658d850 | 73 | void sound_thread(){ |
CRaslawski | 4:589e4a2028a7 | 74 | while(1) { |
CRaslawski | 4:589e4a2028a7 | 75 | //FILE *wave_file; |
CRaslawski | 4:589e4a2028a7 | 76 | wave_file=fopen("/sd/vacuum.wav","r"); |
CRaslawski | 4:589e4a2028a7 | 77 | if (wave_file == NULL){ |
CRaslawski | 6:21c5a7cf63de | 78 | led1=led4 = 1; // if file read error, outside LEDs are on |
CRaslawski | 6:21c5a7cf63de | 79 | led2=led3 = 0; |
CRaslawski | 4:589e4a2028a7 | 80 | } |
CRaslawski | 4:589e4a2028a7 | 81 | waver.play(wave_file); |
CRaslawski | 4:589e4a2028a7 | 82 | fclose(wave_file); |
jplager3 | 0:ada50658d850 | 83 | } |
jplager3 | 0:ada50658d850 | 84 | } |
CRaslawski | 5:6ca141fc6c4e | 85 | |
CRaslawski | 6:21c5a7cf63de | 86 | void beep(){ |
CRaslawski | 6:21c5a7cf63de | 87 | wave_file2=fopen("/sd/beep.wav","r"); |
CRaslawski | 6:21c5a7cf63de | 88 | if(wave_file2 == NULL) { |
CRaslawski | 6:21c5a7cf63de | 89 | led1=led4 = 0; // if file read error, inside LEDs are on |
CRaslawski | 6:21c5a7cf63de | 90 | led2=led3 = 0; |
CRaslawski | 6:21c5a7cf63de | 91 | } |
CRaslawski | 6:21c5a7cf63de | 92 | waver.play(wave_file2); |
CRaslawski | 6:21c5a7cf63de | 93 | fclose(wave_file2); |
CRaslawski | 6:21c5a7cf63de | 94 | } |
jplager3 | 9:37bb5f0e2975 | 95 | void drive(float speed){ |
jplager3 | 9:37bb5f0e2975 | 96 | Left.speed(speed); |
jplager3 | 9:37bb5f0e2975 | 97 | Right.speed(speed); |
jplager3 | 9:37bb5f0e2975 | 98 | } |
jplager3 | 9:37bb5f0e2975 | 99 | void reverse(float speed){ |
jplager3 | 9:37bb5f0e2975 | 100 | Left.speed(-speed); |
jplager3 | 9:37bb5f0e2975 | 101 | Right.speed(-speed); |
jplager3 | 9:37bb5f0e2975 | 102 | } |
jplager3 | 9:37bb5f0e2975 | 103 | void turnRight(float speed){ |
jplager3 | 9:37bb5f0e2975 | 104 | Left.speed(speed); |
jplager3 | 9:37bb5f0e2975 | 105 | Right.speed(-speed); |
jplager3 | 9:37bb5f0e2975 | 106 | //wait(0.7); |
jplager3 | 9:37bb5f0e2975 | 107 | } |
jplager3 | 9:37bb5f0e2975 | 108 | void turnLeft(float speed){ |
jplager3 | 9:37bb5f0e2975 | 109 | Left.speed(-speed); |
jplager3 | 9:37bb5f0e2975 | 110 | Right.speed(speed); |
jplager3 | 9:37bb5f0e2975 | 111 | //wait(0.7); |
jplager3 | 9:37bb5f0e2975 | 112 | } |
jplager3 | 9:37bb5f0e2975 | 113 | void stop(){ |
jplager3 | 9:37bb5f0e2975 | 114 | Left.speed(0.0); |
jplager3 | 9:37bb5f0e2975 | 115 | Right.speed(0.0); |
jplager3 | 9:37bb5f0e2975 | 116 | } |
jplager3 | 9:37bb5f0e2975 | 117 | |
jplager3 | 0:ada50658d850 | 118 | int main() { |
jplager3 | 9:37bb5f0e2975 | 119 | Left.speed(0.0); //attempt at forcing Left to chill on startup. ineffective. |
jplager3 | 9:37bb5f0e2975 | 120 | Right.speed(0.0); //Right does not have the issue. Is chill on startup |
jplager3 | 0:ada50658d850 | 121 | //thread1.start(IR_thread); // read in IR data |
jplager3 | 10:059154e51621 | 122 | //thread2.start(LCD_thread1); |
jplager3 | 0:ada50658d850 | 123 | //thread3.start(Motor_thread); |
jplager3 | 0:ada50658d850 | 124 | thread4.start(sound_thread); |
jplager3 | 0:ada50658d850 | 125 | |
jplager3 | 9:37bb5f0e2975 | 126 | float turnDirectionSelector = 0.3; |
jplager3 | 9:37bb5f0e2975 | 127 | drive(0.3); // begin driving fwd |
jplager3 | 10:059154e51621 | 128 | //thread1.start(LCD_GRN); // I think when thread closes, LCD reverts to black. |
jplager3 | 10:059154e51621 | 129 | LCD_GRN(); |
jplager3 | 10:059154e51621 | 130 | //bool screenNotWritten = false; |
jplager3 | 9:37bb5f0e2975 | 131 | while(1){ |
CRaslawski | 5:6ca141fc6c4e | 132 | //IR testing |
jplager3 | 10:059154e51621 | 133 | if(IR>0.3 && IR<0.5) { |
jplager3 | 10:059154e51621 | 134 | LCD_GRN(); // scan for object |
CRaslawski | 5:6ca141fc6c4e | 135 | led1=1; |
CRaslawski | 5:6ca141fc6c4e | 136 | led2=led3=led4=0; |
CRaslawski | 5:6ca141fc6c4e | 137 | } else if (IR>0.5 && IR< 0.7) { |
jplager3 | 10:059154e51621 | 138 | //thread2.start(LCD_YEL); |
jplager3 | 10:059154e51621 | 139 | LCD_GRN(); |
CRaslawski | 5:6ca141fc6c4e | 140 | led1=led2=1; |
CRaslawski | 5:6ca141fc6c4e | 141 | led3=led4=0; |
CRaslawski | 5:6ca141fc6c4e | 142 | } else if (IR>0.7 && IR<0.9) { |
jplager3 | 10:059154e51621 | 143 | LCD_YEL(); |
CRaslawski | 5:6ca141fc6c4e | 144 | led1=led2=led3=1; |
CRaslawski | 5:6ca141fc6c4e | 145 | led4=0; |
CRaslawski | 6:21c5a7cf63de | 146 | } else if(IR>0.9){ //collision threshold seems to be the same from IR=0.8 to IR=0.95 |
jplager3 | 9:37bb5f0e2975 | 147 | // in this block, "collision" detected. Back up, execute turn, and proceed driving fwd. |
jplager3 | 10:059154e51621 | 148 | //thread3.start(LCD_RED); |
jplager3 | 10:059154e51621 | 149 | |
jplager3 | 10:059154e51621 | 150 | //LCD_RED(); |
CRaslawski | 6:21c5a7cf63de | 151 | led1=led2=led3=led4=1; |
CRaslawski | 6:21c5a7cf63de | 152 | fclose(wave_file); |
CRaslawski | 6:21c5a7cf63de | 153 | thread4.terminate(); |
CRaslawski | 6:21c5a7cf63de | 154 | thread4.start(beep); |
jplager3 | 9:37bb5f0e2975 | 155 | // reverse motors and turn Roomba here |
jplager3 | 9:37bb5f0e2975 | 156 | stop(); |
jplager3 | 9:37bb5f0e2975 | 157 | wait(0.4); |
jplager3 | 9:37bb5f0e2975 | 158 | reverse(0.3); |
jplager3 | 9:37bb5f0e2975 | 159 | wait(2); |
jplager3 | 9:37bb5f0e2975 | 160 | turnDirectionSelector*=-1; // will alternate turning left/right upon collision |
jplager3 | 9:37bb5f0e2975 | 161 | turnRight(turnDirectionSelector); |
jplager3 | 9:37bb5f0e2975 | 162 | wait(1); //2s is a bit more than 180deg @ 0.3 speed |
jplager3 | 9:37bb5f0e2975 | 163 | stop(); |
jplager3 | 9:37bb5f0e2975 | 164 | |
jplager3 | 10:059154e51621 | 165 | wait(0.7); //wait for beeping to finish |
jplager3 | 10:059154e51621 | 166 | |
CRaslawski | 6:21c5a7cf63de | 167 | fclose(wave_file2); |
CRaslawski | 6:21c5a7cf63de | 168 | thread4.terminate(); |
CRaslawski | 6:21c5a7cf63de | 169 | thread4.start(sound_thread); |
jplager3 | 9:37bb5f0e2975 | 170 | drive(0.4); |
jplager3 | 10:059154e51621 | 171 | //thread1.start(LCD_GRN); |
CRaslawski | 5:6ca141fc6c4e | 172 | } else led1=led2=led3=led4=0; |
jplager3 | 0:ada50658d850 | 173 | } |
jplager3 | 0:ada50658d850 | 174 | } |