Complete Build
Dependencies: 4DGL-uLCD-SE1 Motor SDFileSystem X_NUCLEO_53L0A1 mbed-rtos mbed BotwithWavePlayerLevel
Fork of BotWithBluetoothLIDARV2 by
main.cpp@17:2c6549be1ecb, 2017-11-20 (annotated)
- Committer:
- ctallardc
- Date:
- Mon Nov 20 01:47:29 2017 +0000
- Revision:
- 17:2c6549be1ecb
- Parent:
- 16:61e88b0e3689
- Child:
- 18:aa7f48962bdb
First Version BotWithLIDAR Working
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
bdragon52 | 16:61e88b0e3689 | 1 | |
emilmont | 1:491820ee784d | 2 | #include "mbed.h" |
bdragon52 | 16:61e88b0e3689 | 3 | #include "Motor.h" |
mbed_official | 11:0309bef74ba8 | 4 | #include "rtos.h" |
bdragon52 | 12:af37c8938aaf | 5 | |
ctallardc | 17:2c6549be1ecb | 6 | #define SD_DBG 1 |
ctallardc | 17:2c6549be1ecb | 7 | |
bdragon52 | 16:61e88b0e3689 | 8 | #include "SDFileSystem.h" |
bdragon52 | 16:61e88b0e3689 | 9 | SDFileSystem sd(p5, p6, p7, p29, "sd"); |
bdragon52 | 12:af37c8938aaf | 10 | |
bdragon52 | 16:61e88b0e3689 | 11 | |
bdragon52 | 16:61e88b0e3689 | 12 | #include "wave_player.h" |
bdragon52 | 12:af37c8938aaf | 13 | AnalogOut DACout(p18); |
bdragon52 | 12:af37c8938aaf | 14 | wave_player waver(&DACout); |
bdragon52 | 16:61e88b0e3689 | 15 | |
ctallardc | 17:2c6549be1ecb | 16 | PwmOut speaker(p23); |
bdragon52 | 15:c8360ceaad61 | 17 | |
bdragon52 | 16:61e88b0e3689 | 18 | //BusOut myled(LED1,LED2,LED3,LED4); |
bdragon52 | 16:61e88b0e3689 | 19 | #include "XNucleo53L0A1.h" |
bdragon52 | 16:61e88b0e3689 | 20 | #include <stdio.h> |
bdragon52 | 16:61e88b0e3689 | 21 | Serial pc(USBTX,USBRX); |
bdragon52 | 16:61e88b0e3689 | 22 | DigitalOut shdn(p26); |
bdragon52 | 16:61e88b0e3689 | 23 | //I2C sensor pins |
bdragon52 | 16:61e88b0e3689 | 24 | #define VL53L0_I2C_SDA p28 |
bdragon52 | 16:61e88b0e3689 | 25 | #define VL53L0_I2C_SCL p27 |
bdragon52 | 16:61e88b0e3689 | 26 | static XNucleo53L0A1 *board=NULL; |
bdragon52 | 16:61e88b0e3689 | 27 | DigitalOut myled (LED2); |
bdragon52 | 16:61e88b0e3689 | 28 | Serial blue(p9,p10); |
bdragon52 | 16:61e88b0e3689 | 29 | Motor RW(p22, p13, p12); // pwm, fwd, rev |
bdragon52 | 16:61e88b0e3689 | 30 | Motor LW(p21, p8, p11); // pwm, fwd, rev |
bdragon52 | 16:61e88b0e3689 | 31 | Thread lidar; |
bdragon52 | 16:61e88b0e3689 | 32 | Thread siren; |
bdragon52 | 16:61e88b0e3689 | 33 | uint32_t distance; |
ctallardc | 17:2c6549be1ecb | 34 | int level = 8; //Volume level is turned off |
ctallardc | 17:2c6549be1ecb | 35 | //wave_file; |
ctallardc | 17:2c6549be1ecb | 36 | |
ctallardc | 17:2c6549be1ecb | 37 | void listdir(void) { |
ctallardc | 17:2c6549be1ecb | 38 | DIR *d; |
ctallardc | 17:2c6549be1ecb | 39 | struct dirent *p; |
cjtallu | 14:cc2e4fbaffd8 | 40 | |
ctallardc | 17:2c6549be1ecb | 41 | d = opendir("/sd"); |
ctallardc | 17:2c6549be1ecb | 42 | if (d != NULL) { |
ctallardc | 17:2c6549be1ecb | 43 | while ((p = readdir(d)) != NULL) { |
ctallardc | 17:2c6549be1ecb | 44 | pc.printf(" - %s\r\n", p->d_name); |
ctallardc | 17:2c6549be1ecb | 45 | } |
ctallardc | 17:2c6549be1ecb | 46 | } else { |
ctallardc | 17:2c6549be1ecb | 47 | pc.printf("Could not open directory!\r\n"); |
ctallardc | 17:2c6549be1ecb | 48 | } |
ctallardc | 17:2c6549be1ecb | 49 | closedir(d); |
ctallardc | 17:2c6549be1ecb | 50 | } |
bdragon52 | 16:61e88b0e3689 | 51 | |
bdragon52 | 16:61e88b0e3689 | 52 | void t3(){ |
ctallardc | 17:2c6549be1ecb | 53 | // int i; |
ctallardc | 17:2c6549be1ecb | 54 | pc.printf("Speaker\r\n"); |
ctallardc | 17:2c6549be1ecb | 55 | listdir(); |
ctallardc | 17:2c6549be1ecb | 56 | //waver.set_verbosity(1); |
ctallardc | 17:2c6549be1ecb | 57 | // FILE *fp = fopen("/sd/myfile2.txt", "w"); |
ctallardc | 17:2c6549be1ecb | 58 | // fprintf(fp, "Hello World 11/18\n"); |
ctallardc | 17:2c6549be1ecb | 59 | // fclose(fp); |
bdragon52 | 16:61e88b0e3689 | 60 | while(1){ |
ctallardc | 17:2c6549be1ecb | 61 | |
ctallardc | 17:2c6549be1ecb | 62 | |
ctallardc | 17:2c6549be1ecb | 63 | /* generate a 500Hz tone using PWM hardware output |
ctallardc | 17:2c6549be1ecb | 64 | speaker.period(1.0/500.0); // 500hz period |
ctallardc | 17:2c6549be1ecb | 65 | speaker =0.5; //50% duty cycle - max volume |
ctallardc | 17:2c6549be1ecb | 66 | wait(3); |
ctallardc | 17:2c6549be1ecb | 67 | speaker=0.0; // turn off audio |
ctallardc | 17:2c6549be1ecb | 68 | wait(2); |
ctallardc | 17:2c6549be1ecb | 69 | |
ctallardc | 17:2c6549be1ecb | 70 | // generate a short 150Hz tone using PWM hardware output |
ctallardc | 17:2c6549be1ecb | 71 | // something like this can be used for a button click effect for feedback |
ctallardc | 17:2c6549be1ecb | 72 | for (i=0; i<10; i++) { |
ctallardc | 17:2c6549be1ecb | 73 | speaker.period(1.0/150.0); // 500hz period |
ctallardc | 17:2c6549be1ecb | 74 | speaker =0.25; //25% duty cycle - mid range volume |
ctallardc | 17:2c6549be1ecb | 75 | wait(.02); |
ctallardc | 17:2c6549be1ecb | 76 | speaker=0.0; // turn off audio |
ctallardc | 17:2c6549be1ecb | 77 | wait(0.5); |
ctallardc | 17:2c6549be1ecb | 78 | } |
ctallardc | 17:2c6549be1ecb | 79 | */ |
ctallardc | 17:2c6549be1ecb | 80 | while(1){ |
ctallardc | 17:2c6549be1ecb | 81 | FILE *wave_file=fopen("/sd/LoudSound.wav","r"); |
ctallardc | 17:2c6549be1ecb | 82 | |
bdragon52 | 16:61e88b0e3689 | 83 | waver.play(wave_file); |
ctallardc | 17:2c6549be1ecb | 84 | |
bdragon52 | 16:61e88b0e3689 | 85 | fclose(wave_file); |
ctallardc | 17:2c6549be1ecb | 86 | //Thread::wait(50); |
ctallardc | 17:2c6549be1ecb | 87 | } |
bdragon52 | 12:af37c8938aaf | 88 | } |
cjtallu | 14:cc2e4fbaffd8 | 89 | } |
cjtallu | 14:cc2e4fbaffd8 | 90 | |
bdragon52 | 16:61e88b0e3689 | 91 | void t2() { |
bdragon52 | 16:61e88b0e3689 | 92 | |
bdragon52 | 16:61e88b0e3689 | 93 | int status; |
bdragon52 | 16:61e88b0e3689 | 94 | DevI2C *device_i2c = new DevI2C(VL53L0_I2C_SDA, VL53L0_I2C_SCL); |
bdragon52 | 16:61e88b0e3689 | 95 | /* creates the 53L0A1 expansion board singleton obj */ |
bdragon52 | 16:61e88b0e3689 | 96 | board = XNucleo53L0A1::instance(device_i2c, A2, D8, D2); |
bdragon52 | 16:61e88b0e3689 | 97 | shdn = 0; //must reset sensor for an mbed reset to work |
bdragon52 | 16:61e88b0e3689 | 98 | Thread::wait(100); |
ctallardc | 17:2c6549be1ecb | 99 | //lidar power off |
bdragon52 | 16:61e88b0e3689 | 100 | shdn = 1; |
bdragon52 | 16:61e88b0e3689 | 101 | Thread::wait(100); |
bdragon52 | 16:61e88b0e3689 | 102 | /* init the 53L0A1 board with default values */ |
bdragon52 | 16:61e88b0e3689 | 103 | status = board->init_board(); |
bdragon52 | 16:61e88b0e3689 | 104 | while (status) { |
bdragon52 | 16:61e88b0e3689 | 105 | pc.printf("Failed to init board! \r\n"); |
bdragon52 | 16:61e88b0e3689 | 106 | status = board->init_board(); |
emilmont | 1:491820ee784d | 107 | } |
bdragon52 | 16:61e88b0e3689 | 108 | //loop taking and printing distance |
bdragon52 | 13:ab6bbdd2447a | 109 | |
bdragon52 | 16:61e88b0e3689 | 110 | bool speakerOn=false; |
bdragon52 | 16:61e88b0e3689 | 111 | |
bdragon52 | 16:61e88b0e3689 | 112 | while (1) { |
bdragon52 | 16:61e88b0e3689 | 113 | status = board->sensor_centre->get_distance(&distance); |
bdragon52 | 16:61e88b0e3689 | 114 | if (status == VL53L0X_ERROR_NONE) { |
ctallardc | 17:2c6549be1ecb | 115 | pc.printf("D=%ld mm\r\n", distance); |
ctallardc | 17:2c6549be1ecb | 116 | if(distance > 240) level = 8; |
ctallardc | 17:2c6549be1ecb | 117 | if(distance < 90) level = 0; |
ctallardc | 17:2c6549be1ecb | 118 | if (distance >=90 && distance <=240){ |
ctallardc | 17:2c6549be1ecb | 119 | int value=floor(static_cast<double>(8*(distance-90)/150)); |
ctallardc | 17:2c6549be1ecb | 120 | if(value >=0 && value<=8){ |
ctallardc | 17:2c6549be1ecb | 121 | level=value; |
ctallardc | 17:2c6549be1ecb | 122 | } |
ctallardc | 17:2c6549be1ecb | 123 | } |
bdragon52 | 16:61e88b0e3689 | 124 | } |
bdragon52 | 16:61e88b0e3689 | 125 | |
bdragon52 | 16:61e88b0e3689 | 126 | //LIDAR checks |
bdragon52 | 13:ab6bbdd2447a | 127 | |
bdragon52 | 16:61e88b0e3689 | 128 | /* if(speakerOn==false && distance<=100) //thread start sound, speakerOn=true; |
bdragon52 | 16:61e88b0e3689 | 129 | if(speakerOn==true && distance<=100) //certain volume |
bdragon52 | 16:61e88b0e3689 | 130 | if(speakerOn==true && distance<50) //higher volume; |
bdragon52 | 16:61e88b0e3689 | 131 | if(distance>100) //thread stop, speakerOn==false; |
bdragon52 | 16:61e88b0e3689 | 132 | */ |
bdragon52 | 16:61e88b0e3689 | 133 | //siren.start(t3); |
bdragon52 | 16:61e88b0e3689 | 134 | myled=!myled; |
bdragon52 | 16:61e88b0e3689 | 135 | Thread::wait(500); |
bdragon52 | 16:61e88b0e3689 | 136 | } |
bdragon52 | 13:ab6bbdd2447a | 137 | |
bdragon52 | 13:ab6bbdd2447a | 138 | } |
bdragon52 | 13:ab6bbdd2447a | 139 | |
bdragon52 | 13:ab6bbdd2447a | 140 | |
bdragon52 | 13:ab6bbdd2447a | 141 | |
bdragon52 | 16:61e88b0e3689 | 142 | |
bdragon52 | 15:c8360ceaad61 | 143 | |
bdragon52 | 16:61e88b0e3689 | 144 | int main() |
bdragon52 | 16:61e88b0e3689 | 145 | { |
ctallardc | 17:2c6549be1ecb | 146 | char previousButton='0'; |
ctallardc | 17:2c6549be1ecb | 147 | char current; |
bdragon52 | 16:61e88b0e3689 | 148 | myled=0; |
ctallardc | 17:2c6549be1ecb | 149 | pc.printf("in main\n\r"); |
ctallardc | 17:2c6549be1ecb | 150 | lidar.start(t2); |
ctallardc | 17:2c6549be1ecb | 151 | siren.start(t3); |
bdragon52 | 16:61e88b0e3689 | 152 | char bnum=0; |
bdragon52 | 16:61e88b0e3689 | 153 | char bhit=0; |
bdragon52 | 12:af37c8938aaf | 154 | while(1) { |
bdragon52 | 16:61e88b0e3689 | 155 | |
bdragon52 | 16:61e88b0e3689 | 156 | if(blue.readable()==1){ |
bdragon52 | 16:61e88b0e3689 | 157 | if (blue.getc()=='!') { |
bdragon52 | 16:61e88b0e3689 | 158 | if (blue.getc()=='B') { //button data packet |
bdragon52 | 16:61e88b0e3689 | 159 | bnum = blue.getc(); //button number |
bdragon52 | 16:61e88b0e3689 | 160 | bhit = blue.getc(); //1=hit, 0=release |
ctallardc | 17:2c6549be1ecb | 161 | current=bnum; |
ctallardc | 17:2c6549be1ecb | 162 | //Thread::wait(250); |
ctallardc | 17:2c6549be1ecb | 163 | //pc.printf("Button Pressed %c bhit = %c PreviousButton = %c CurrentButton = %c\n\r", bnum, bhit, previousButton, current); |
ctallardc | 17:2c6549be1ecb | 164 | previousButton = current; |
bdragon52 | 16:61e88b0e3689 | 165 | if (blue.getc()==char(~('!' + 'B' + bnum + bhit))) { //checksum OK? |
bdragon52 | 16:61e88b0e3689 | 166 | //myled = bnum - '0'; //current button number will appear on LEDs |
bdragon52 | 16:61e88b0e3689 | 167 | switch (bnum) { |
bdragon52 | 16:61e88b0e3689 | 168 | case '1': //number button 1 |
bdragon52 | 16:61e88b0e3689 | 169 | if (bhit=='1') { |
bdragon52 | 16:61e88b0e3689 | 170 | RW.speed(1); |
bdragon52 | 16:61e88b0e3689 | 171 | //add hit code here |
bdragon52 | 16:61e88b0e3689 | 172 | } else { |
bdragon52 | 16:61e88b0e3689 | 173 | //add release code here |
bdragon52 | 16:61e88b0e3689 | 174 | } |
bdragon52 | 16:61e88b0e3689 | 175 | break; |
bdragon52 | 16:61e88b0e3689 | 176 | case '2': //number button 2 |
bdragon52 | 16:61e88b0e3689 | 177 | if (bhit=='1') { |
bdragon52 | 16:61e88b0e3689 | 178 | RW.speed(0); |
bdragon52 | 16:61e88b0e3689 | 179 | LW.speed(0); |
bdragon52 | 16:61e88b0e3689 | 180 | //add hit code here |
bdragon52 | 16:61e88b0e3689 | 181 | } else { |
bdragon52 | 16:61e88b0e3689 | 182 | //add release code here |
bdragon52 | 16:61e88b0e3689 | 183 | } |
bdragon52 | 16:61e88b0e3689 | 184 | break; |
bdragon52 | 16:61e88b0e3689 | 185 | case '3': //number button 3 |
bdragon52 | 16:61e88b0e3689 | 186 | if (bhit=='1') { |
bdragon52 | 16:61e88b0e3689 | 187 | //add hit code here |
bdragon52 | 16:61e88b0e3689 | 188 | } else { |
bdragon52 | 16:61e88b0e3689 | 189 | //add release code here |
bdragon52 | 16:61e88b0e3689 | 190 | } |
bdragon52 | 16:61e88b0e3689 | 191 | break; |
bdragon52 | 16:61e88b0e3689 | 192 | case '4': //number button 4 |
bdragon52 | 16:61e88b0e3689 | 193 | if (bhit=='1') { |
bdragon52 | 16:61e88b0e3689 | 194 | //add hit code here |
bdragon52 | 16:61e88b0e3689 | 195 | } else { |
bdragon52 | 16:61e88b0e3689 | 196 | //add release code here |
bdragon52 | 16:61e88b0e3689 | 197 | } |
bdragon52 | 16:61e88b0e3689 | 198 | break; |
bdragon52 | 16:61e88b0e3689 | 199 | case '5': //button 5 up arrow |
bdragon52 | 16:61e88b0e3689 | 200 | if (bhit=='1') { |
bdragon52 | 16:61e88b0e3689 | 201 | //add hit code here |
bdragon52 | 16:61e88b0e3689 | 202 | |
bdragon52 | 16:61e88b0e3689 | 203 | RW.speed(1.0); |
bdragon52 | 16:61e88b0e3689 | 204 | LW.speed(1.0); |
bdragon52 | 16:61e88b0e3689 | 205 | |
bdragon52 | 16:61e88b0e3689 | 206 | } else { |
bdragon52 | 16:61e88b0e3689 | 207 | |
bdragon52 | 16:61e88b0e3689 | 208 | LW.speed(0); |
bdragon52 | 16:61e88b0e3689 | 209 | RW.speed(0); |
bdragon52 | 16:61e88b0e3689 | 210 | //add release code here |
bdragon52 | 16:61e88b0e3689 | 211 | } |
bdragon52 | 16:61e88b0e3689 | 212 | break; |
bdragon52 | 16:61e88b0e3689 | 213 | case '6': //button 6 down arrow |
bdragon52 | 16:61e88b0e3689 | 214 | if (bhit=='1') { |
bdragon52 | 16:61e88b0e3689 | 215 | RW.speed(-1.0); |
bdragon52 | 16:61e88b0e3689 | 216 | LW.speed(-1.0); |
bdragon52 | 16:61e88b0e3689 | 217 | //add hit code here |
bdragon52 | 16:61e88b0e3689 | 218 | } else { |
bdragon52 | 16:61e88b0e3689 | 219 | RW.speed(0); |
bdragon52 | 16:61e88b0e3689 | 220 | LW.speed(0); |
bdragon52 | 16:61e88b0e3689 | 221 | //add release code here |
bdragon52 | 16:61e88b0e3689 | 222 | } |
bdragon52 | 16:61e88b0e3689 | 223 | break; |
bdragon52 | 16:61e88b0e3689 | 224 | case '7': //button 7 left arrow |
bdragon52 | 16:61e88b0e3689 | 225 | if (bhit=='1') { |
ctallardc | 17:2c6549be1ecb | 226 | RW.speed(-1.0); |
ctallardc | 17:2c6549be1ecb | 227 | LW.speed(1.0); |
bdragon52 | 16:61e88b0e3689 | 228 | //add hit code here |
bdragon52 | 16:61e88b0e3689 | 229 | } else { |
bdragon52 | 16:61e88b0e3689 | 230 | |
bdragon52 | 16:61e88b0e3689 | 231 | RW.speed(0); |
bdragon52 | 16:61e88b0e3689 | 232 | LW.speed(0); |
bdragon52 | 16:61e88b0e3689 | 233 | //add release code here |
bdragon52 | 16:61e88b0e3689 | 234 | } |
bdragon52 | 16:61e88b0e3689 | 235 | break; |
bdragon52 | 16:61e88b0e3689 | 236 | case '8': //button 8 right arrow |
bdragon52 | 16:61e88b0e3689 | 237 | if (bhit=='1') { |
ctallardc | 17:2c6549be1ecb | 238 | RW.speed(1.0); |
ctallardc | 17:2c6549be1ecb | 239 | LW.speed(-1.0); |
bdragon52 | 16:61e88b0e3689 | 240 | //add hit code here |
bdragon52 | 16:61e88b0e3689 | 241 | } else { |
bdragon52 | 16:61e88b0e3689 | 242 | RW.speed(0); |
bdragon52 | 16:61e88b0e3689 | 243 | LW.speed(0); |
bdragon52 | 16:61e88b0e3689 | 244 | //add release code here |
bdragon52 | 16:61e88b0e3689 | 245 | } |
bdragon52 | 16:61e88b0e3689 | 246 | break; |
bdragon52 | 16:61e88b0e3689 | 247 | default: |
bdragon52 | 16:61e88b0e3689 | 248 | break; |
bdragon52 | 16:61e88b0e3689 | 249 | } |
bdragon52 | 16:61e88b0e3689 | 250 | } |
bdragon52 | 16:61e88b0e3689 | 251 | } |
bdragon52 | 16:61e88b0e3689 | 252 | } |
bdragon52 | 16:61e88b0e3689 | 253 | |
bdragon52 | 16:61e88b0e3689 | 254 | } |
bdragon52 | 16:61e88b0e3689 | 255 | |
bdragon52 | 16:61e88b0e3689 | 256 | Thread::yield(); |
bdragon52 | 16:61e88b0e3689 | 257 | |
bdragon52 | 12:af37c8938aaf | 258 | } |
bdragon52 | 12:af37c8938aaf | 259 | |
bdragon52 | 16:61e88b0e3689 | 260 | } |