Robot that currently does nothing
Dependencies: 4DGL-uLCD-SE SDFileSystem ShiftBrite mbed-rtos mbed wave_player Motor
main.cpp@9:37bb5f0e2975, 2017-03-16 (annotated)
- Committer:
- jplager3
- Date:
- Thu Mar 16 03:39:30 2017 +0000
- Revision:
- 9:37bb5f0e2975
- Parent:
- 8:cde09ae38be7
- Child:
- 10:059154e51621
Functionality added: Roomba drives fwd until collision detected, executes ~90deg L/R turn (alternates direction), then resumes driving fwd with loop.
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 | 0:ada50658d850 | 9 | Mutex mutex; // |
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 | 0:ada50658d850 | 43 | } |
jplager3 | 0:ada50658d850 | 44 | void sound_thread(){ |
CRaslawski | 4:589e4a2028a7 | 45 | while(1) { |
CRaslawski | 4:589e4a2028a7 | 46 | //FILE *wave_file; |
CRaslawski | 4:589e4a2028a7 | 47 | wave_file=fopen("/sd/vacuum.wav","r"); |
CRaslawski | 4:589e4a2028a7 | 48 | if (wave_file == NULL){ |
CRaslawski | 6:21c5a7cf63de | 49 | led1=led4 = 1; // if file read error, outside LEDs are on |
CRaslawski | 6:21c5a7cf63de | 50 | led2=led3 = 0; |
CRaslawski | 4:589e4a2028a7 | 51 | } |
CRaslawski | 4:589e4a2028a7 | 52 | waver.play(wave_file); |
CRaslawski | 4:589e4a2028a7 | 53 | fclose(wave_file); |
jplager3 | 0:ada50658d850 | 54 | } |
jplager3 | 0:ada50658d850 | 55 | } |
CRaslawski | 5:6ca141fc6c4e | 56 | |
CRaslawski | 6:21c5a7cf63de | 57 | void beep(){ |
CRaslawski | 6:21c5a7cf63de | 58 | wave_file2=fopen("/sd/beep.wav","r"); |
CRaslawski | 6:21c5a7cf63de | 59 | if(wave_file2 == NULL) { |
CRaslawski | 6:21c5a7cf63de | 60 | led1=led4 = 0; // if file read error, inside LEDs are on |
CRaslawski | 6:21c5a7cf63de | 61 | led2=led3 = 0; |
CRaslawski | 6:21c5a7cf63de | 62 | } |
CRaslawski | 6:21c5a7cf63de | 63 | waver.play(wave_file2); |
CRaslawski | 6:21c5a7cf63de | 64 | fclose(wave_file2); |
CRaslawski | 6:21c5a7cf63de | 65 | } |
jplager3 | 9:37bb5f0e2975 | 66 | void drive(float speed){ |
jplager3 | 9:37bb5f0e2975 | 67 | Left.speed(speed); |
jplager3 | 9:37bb5f0e2975 | 68 | Right.speed(speed); |
jplager3 | 9:37bb5f0e2975 | 69 | } |
jplager3 | 9:37bb5f0e2975 | 70 | void reverse(float speed){ |
jplager3 | 9:37bb5f0e2975 | 71 | Left.speed(-speed); |
jplager3 | 9:37bb5f0e2975 | 72 | Right.speed(-speed); |
jplager3 | 9:37bb5f0e2975 | 73 | } |
jplager3 | 9:37bb5f0e2975 | 74 | void turnRight(float speed){ |
jplager3 | 9:37bb5f0e2975 | 75 | Left.speed(speed); |
jplager3 | 9:37bb5f0e2975 | 76 | Right.speed(-speed); |
jplager3 | 9:37bb5f0e2975 | 77 | //wait(0.7); |
jplager3 | 9:37bb5f0e2975 | 78 | } |
jplager3 | 9:37bb5f0e2975 | 79 | void turnLeft(float speed){ |
jplager3 | 9:37bb5f0e2975 | 80 | Left.speed(-speed); |
jplager3 | 9:37bb5f0e2975 | 81 | Right.speed(speed); |
jplager3 | 9:37bb5f0e2975 | 82 | //wait(0.7); |
jplager3 | 9:37bb5f0e2975 | 83 | } |
jplager3 | 9:37bb5f0e2975 | 84 | void stop(){ |
jplager3 | 9:37bb5f0e2975 | 85 | Left.speed(0.0); |
jplager3 | 9:37bb5f0e2975 | 86 | Right.speed(0.0); |
jplager3 | 9:37bb5f0e2975 | 87 | } |
jplager3 | 9:37bb5f0e2975 | 88 | |
CRaslawski | 6:21c5a7cf63de | 89 | |
jplager3 | 0:ada50658d850 | 90 | int main() { |
jplager3 | 9:37bb5f0e2975 | 91 | Left.speed(0.0); //attempt at forcing Left to chill on startup. ineffective. |
jplager3 | 9:37bb5f0e2975 | 92 | Right.speed(0.0); //Right does not have the issue. Is chill on startup |
jplager3 | 0:ada50658d850 | 93 | //thread1.start(IR_thread); // read in IR data |
jplager3 | 0:ada50658d850 | 94 | thread2.start(LCD_thread1); |
jplager3 | 0:ada50658d850 | 95 | //thread3.start(Motor_thread); |
jplager3 | 0:ada50658d850 | 96 | thread4.start(sound_thread); |
jplager3 | 0:ada50658d850 | 97 | |
jplager3 | 9:37bb5f0e2975 | 98 | float turnDirectionSelector = 0.3; |
jplager3 | 9:37bb5f0e2975 | 99 | drive(0.3); // begin driving fwd |
jplager3 | 9:37bb5f0e2975 | 100 | while(1){ |
CRaslawski | 5:6ca141fc6c4e | 101 | //IR testing |
jplager3 | 9:37bb5f0e2975 | 102 | if(IR>0.3 && IR<0.5) { // scan for object |
CRaslawski | 5:6ca141fc6c4e | 103 | led1=1; |
CRaslawski | 5:6ca141fc6c4e | 104 | led2=led3=led4=0; |
CRaslawski | 5:6ca141fc6c4e | 105 | } else if (IR>0.5 && IR< 0.7) { |
CRaslawski | 5:6ca141fc6c4e | 106 | led1=led2=1; |
CRaslawski | 5:6ca141fc6c4e | 107 | led3=led4=0; |
CRaslawski | 5:6ca141fc6c4e | 108 | } else if (IR>0.7 && IR<0.9) { |
CRaslawski | 5:6ca141fc6c4e | 109 | led1=led2=led3=1; |
CRaslawski | 5:6ca141fc6c4e | 110 | led4=0; |
CRaslawski | 6:21c5a7cf63de | 111 | } else if(IR>0.9){ //collision threshold seems to be the same from IR=0.8 to IR=0.95 |
jplager3 | 9:37bb5f0e2975 | 112 | // in this block, "collision" detected. Back up, execute turn, and proceed driving fwd. |
CRaslawski | 6:21c5a7cf63de | 113 | led1=led2=led3=led4=1; |
CRaslawski | 6:21c5a7cf63de | 114 | fclose(wave_file); |
CRaslawski | 6:21c5a7cf63de | 115 | thread4.terminate(); |
CRaslawski | 6:21c5a7cf63de | 116 | thread4.start(beep); |
jplager3 | 9:37bb5f0e2975 | 117 | // reverse motors and turn Roomba here |
jplager3 | 9:37bb5f0e2975 | 118 | stop(); |
jplager3 | 9:37bb5f0e2975 | 119 | wait(0.4); |
jplager3 | 9:37bb5f0e2975 | 120 | reverse(0.3); |
jplager3 | 9:37bb5f0e2975 | 121 | wait(2); |
jplager3 | 9:37bb5f0e2975 | 122 | turnDirectionSelector*=-1; // will alternate turning left/right upon collision |
jplager3 | 9:37bb5f0e2975 | 123 | turnRight(turnDirectionSelector); |
jplager3 | 9:37bb5f0e2975 | 124 | wait(1); //2s is a bit more than 180deg @ 0.3 speed |
jplager3 | 9:37bb5f0e2975 | 125 | stop(); |
jplager3 | 9:37bb5f0e2975 | 126 | |
jplager3 | 9:37bb5f0e2975 | 127 | wait(2); //wait for beeping to finish |
CRaslawski | 6:21c5a7cf63de | 128 | fclose(wave_file2); |
CRaslawski | 6:21c5a7cf63de | 129 | thread4.terminate(); |
CRaslawski | 6:21c5a7cf63de | 130 | thread4.start(sound_thread); |
jplager3 | 9:37bb5f0e2975 | 131 | drive(0.4); |
CRaslawski | 5:6ca141fc6c4e | 132 | } else led1=led2=led3=led4=0; |
jplager3 | 0:ada50658d850 | 133 | } |
jplager3 | 0:ada50658d850 | 134 | } |