![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Bluetooth controlled Robot with Smart Collision Detection for ECE 4180 Final Project
Dependencies: Servo X_NUCLEO_53L0A1 mbed-rtos mbed
main.cpp@0:044f44184170, 2018-04-27 (annotated)
- Committer:
- dchase
- Date:
- Fri Apr 27 20:03:14 2018 +0000
- Revision:
- 0:044f44184170
Final Revision
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
dchase | 0:044f44184170 | 1 | #include "mbed.h" |
dchase | 0:044f44184170 | 2 | #include "XNucleo53L0A1.h" //Lidar Distance Sensor: 3.3V VIN, GND GND, P13 SDA, P14 SCL, P26 SHDN |
dchase | 0:044f44184170 | 3 | #include "Servo.h" |
dchase | 0:044f44184170 | 4 | #include "rtos.h" |
dchase | 0:044f44184170 | 5 | #include <stdio.h> |
dchase | 0:044f44184170 | 6 | |
dchase | 0:044f44184170 | 7 | BusOut myled(LED1,LED2,LED3,LED4); |
dchase | 0:044f44184170 | 8 | Serial blue(p13,p14); //bluetooth UART |
dchase | 0:044f44184170 | 9 | |
dchase | 0:044f44184170 | 10 | Servo myservo(p23); //Servo connected to PWM 23 |
dchase | 0:044f44184170 | 11 | |
dchase | 0:044f44184170 | 12 | float p; |
dchase | 0:044f44184170 | 13 | PwmOut ledR(p26); //RGB Red |
dchase | 0:044f44184170 | 14 | PwmOut ledG(p25); //RGB Green |
dchase | 0:044f44184170 | 15 | PwmOut ledB(p24); //RGB Blue |
dchase | 0:044f44184170 | 16 | |
dchase | 0:044f44184170 | 17 | PwmOut SpeedL(p21); //Speed of Left motor |
dchase | 0:044f44184170 | 18 | PwmOut SpeedR(p22); //Speed of Right motor |
dchase | 0:044f44184170 | 19 | DigitalOut FwdL(p15); //Forward Left motor |
dchase | 0:044f44184170 | 20 | DigitalOut FwdR(p16); //Forward Right motor |
dchase | 0:044f44184170 | 21 | DigitalOut BckL(p19); //Backward Left motor |
dchase | 0:044f44184170 | 22 | DigitalOut BckR(p18); //Backward Right motor |
dchase | 0:044f44184170 | 23 | |
dchase | 0:044f44184170 | 24 | /* I2C LIDAR sensor */ |
dchase | 0:044f44184170 | 25 | #define VL53L0_I2C_SDA p28 |
dchase | 0:044f44184170 | 26 | #define VL53L0_I2C_SCL p27 |
dchase | 0:044f44184170 | 27 | DevI2C *device_i2c = new DevI2C(VL53L0_I2C_SDA, VL53L0_I2C_SCL); |
dchase | 0:044f44184170 | 28 | static XNucleo53L0A1 *board = XNucleo53L0A1::instance(device_i2c, A2, D8, D2);; // LIDAR singleton obj instantiate |
dchase | 0:044f44184170 | 29 | DigitalOut shdn(p29); |
dchase | 0:044f44184170 | 30 | int status; |
dchase | 0:044f44184170 | 31 | uint32_t sensorData; |
dchase | 0:044f44184170 | 32 | volatile int distance; |
dchase | 0:044f44184170 | 33 | int DistFlag; //used to determine if collision would have occured |
dchase | 0:044f44184170 | 34 | |
dchase | 0:044f44184170 | 35 | Thread t1; |
dchase | 0:044f44184170 | 36 | Thread t2; |
dchase | 0:044f44184170 | 37 | |
dchase | 0:044f44184170 | 38 | void sensor() |
dchase | 0:044f44184170 | 39 | { |
dchase | 0:044f44184170 | 40 | shdn = 0; //must reset sensor for an mbed reset to work |
dchase | 0:044f44184170 | 41 | wait(0.1); |
dchase | 0:044f44184170 | 42 | shdn = 1; |
dchase | 0:044f44184170 | 43 | wait(0.1); |
dchase | 0:044f44184170 | 44 | // init the 53L0A1 board with default values |
dchase | 0:044f44184170 | 45 | status = board->init_board(); |
dchase | 0:044f44184170 | 46 | while (status) { |
dchase | 0:044f44184170 | 47 | pc.printf("Failed to init board! \r\n"); |
dchase | 0:044f44184170 | 48 | status = board->init_board(); |
dchase | 0:044f44184170 | 49 | } |
dchase | 0:044f44184170 | 50 | pc.printf("Board Init. Beginning Loop \r\n"); |
dchase | 0:044f44184170 | 51 | //loop taking and printing distance |
dchase | 0:044f44184170 | 52 | while (1) { |
dchase | 0:044f44184170 | 53 | status = board->sensor_centre->get_distance(&sensorData); |
dchase | 0:044f44184170 | 54 | distance = sensorData; |
dchase | 0:044f44184170 | 55 | if (status == VL53L0X_ERROR_NONE) { |
dchase | 0:044f44184170 | 56 | if (distance <= 400) DistFlag = 1; |
dchase | 0:044f44184170 | 57 | pc.printf("D=%ld mm // DistFlag = %d\r\n", distance, DistFlag); |
dchase | 0:044f44184170 | 58 | } |
dchase | 0:044f44184170 | 59 | Thread::wait(10); |
dchase | 0:044f44184170 | 60 | }//end while |
dchase | 0:044f44184170 | 61 | } // end sensor |
dchase | 0:044f44184170 | 62 | |
dchase | 0:044f44184170 | 63 | void motor() |
dchase | 0:044f44184170 | 64 | { |
dchase | 0:044f44184170 | 65 | char bnum=0; |
dchase | 0:044f44184170 | 66 | char bhit=0; |
dchase | 0:044f44184170 | 67 | while (1) { |
dchase | 0:044f44184170 | 68 | if (DistFlag == 1) { |
dchase | 0:044f44184170 | 69 | ledG = 0; |
dchase | 0:044f44184170 | 70 | ledR = 1; |
dchase | 0:044f44184170 | 71 | SpeedL = 0.5; |
dchase | 0:044f44184170 | 72 | SpeedR = 0.5; |
dchase | 0:044f44184170 | 73 | FwdL = 0; |
dchase | 0:044f44184170 | 74 | FwdR = 0; |
dchase | 0:044f44184170 | 75 | } else { |
dchase | 0:044f44184170 | 76 | ledG = 1; |
dchase | 0:044f44184170 | 77 | if (blue.readable()) { |
dchase | 0:044f44184170 | 78 | if (blue.getc()=='!') { |
dchase | 0:044f44184170 | 79 | if (blue.getc()=='B') { //button data packet |
dchase | 0:044f44184170 | 80 | bnum = blue.getc(); //button number |
dchase | 0:044f44184170 | 81 | bhit = blue.getc(); //1=hit, 0=release |
dchase | 0:044f44184170 | 82 | if (blue.getc()==char(~('!' + 'B' + bnum + bhit))) { //checksum OK? |
dchase | 0:044f44184170 | 83 | myled = bnum - '0'; //current button number will appear on LEDs |
dchase | 0:044f44184170 | 84 | switch (bnum) { |
dchase | 0:044f44184170 | 85 | case '1': //number button 1: Servo Sweep |
dchase | 0:044f44184170 | 86 | if (bhit=='1') { |
dchase | 0:044f44184170 | 87 | for(float p=0; p<1.0; p += 0.1) { |
dchase | 0:044f44184170 | 88 | myservo = p; |
dchase | 0:044f44184170 | 89 | wait(0.2); |
dchase | 0:044f44184170 | 90 | } |
dchase | 0:044f44184170 | 91 | } else { |
dchase | 0:044f44184170 | 92 | myservo = 0.5; |
dchase | 0:044f44184170 | 93 | } |
dchase | 0:044f44184170 | 94 | break; |
dchase | 0:044f44184170 | 95 | case '2': //number button 2 |
dchase | 0:044f44184170 | 96 | if (bhit=='1') { |
dchase | 0:044f44184170 | 97 | //add hit code here |
dchase | 0:044f44184170 | 98 | DistFlag = 0; |
dchase | 0:044f44184170 | 99 | } else { |
dchase | 0:044f44184170 | 100 | //add release code here |
dchase | 0:044f44184170 | 101 | } |
dchase | 0:044f44184170 | 102 | break; |
dchase | 0:044f44184170 | 103 | case '3': //number button 3 |
dchase | 0:044f44184170 | 104 | if (bhit=='1') { |
dchase | 0:044f44184170 | 105 | //add hit code here |
dchase | 0:044f44184170 | 106 | } else { |
dchase | 0:044f44184170 | 107 | //add release code here |
dchase | 0:044f44184170 | 108 | } |
dchase | 0:044f44184170 | 109 | break; |
dchase | 0:044f44184170 | 110 | case '4': //number button 4 |
dchase | 0:044f44184170 | 111 | if (bhit=='1') { |
dchase | 0:044f44184170 | 112 | //add hit code here |
dchase | 0:044f44184170 | 113 | } else { |
dchase | 0:044f44184170 | 114 | //add release code here |
dchase | 0:044f44184170 | 115 | } |
dchase | 0:044f44184170 | 116 | break; |
dchase | 0:044f44184170 | 117 | case '5': //button 5 up arrow |
dchase | 0:044f44184170 | 118 | //pc.printf("Pressed F\n"); |
dchase | 0:044f44184170 | 119 | if (bhit=='1') { |
dchase | 0:044f44184170 | 120 | SpeedL = 1; |
dchase | 0:044f44184170 | 121 | SpeedR = 1; |
dchase | 0:044f44184170 | 122 | FwdL = 1; |
dchase | 0:044f44184170 | 123 | FwdR = 1; |
dchase | 0:044f44184170 | 124 | } else { |
dchase | 0:044f44184170 | 125 | for (p = SpeedL; p > 0; p -= 0.1) { |
dchase | 0:044f44184170 | 126 | SpeedL = p; |
dchase | 0:044f44184170 | 127 | SpeedR = p; |
dchase | 0:044f44184170 | 128 | } |
dchase | 0:044f44184170 | 129 | FwdL = 0; |
dchase | 0:044f44184170 | 130 | FwdR = 0; |
dchase | 0:044f44184170 | 131 | } |
dchase | 0:044f44184170 | 132 | break; |
dchase | 0:044f44184170 | 133 | case '6': //button 6 down arrow |
dchase | 0:044f44184170 | 134 | if (bhit=='1') { |
dchase | 0:044f44184170 | 135 | SpeedL = 0.5; |
dchase | 0:044f44184170 | 136 | SpeedR = 0.5; |
dchase | 0:044f44184170 | 137 | BckL = 1; |
dchase | 0:044f44184170 | 138 | BckR = 1; |
dchase | 0:044f44184170 | 139 | } else { |
dchase | 0:044f44184170 | 140 | for (p = SpeedL; p > 0; p -= 0.1) { |
dchase | 0:044f44184170 | 141 | SpeedL = p; |
dchase | 0:044f44184170 | 142 | SpeedR = p; |
dchase | 0:044f44184170 | 143 | } |
dchase | 0:044f44184170 | 144 | BckL = 0; |
dchase | 0:044f44184170 | 145 | BckR = 0; |
dchase | 0:044f44184170 | 146 | } |
dchase | 0:044f44184170 | 147 | break; |
dchase | 0:044f44184170 | 148 | case '7': //button 7 left arrow |
dchase | 0:044f44184170 | 149 | if (bhit=='1') { |
dchase | 0:044f44184170 | 150 | SpeedL = 0.5; |
dchase | 0:044f44184170 | 151 | SpeedR = 0.5; |
dchase | 0:044f44184170 | 152 | BckL = 1; |
dchase | 0:044f44184170 | 153 | FwdR = 1; |
dchase | 0:044f44184170 | 154 | } else { |
dchase | 0:044f44184170 | 155 | for (p = SpeedL; p > 0; p -= 0.1) { |
dchase | 0:044f44184170 | 156 | SpeedL = p; |
dchase | 0:044f44184170 | 157 | SpeedR = p; |
dchase | 0:044f44184170 | 158 | } |
dchase | 0:044f44184170 | 159 | BckL = 0; |
dchase | 0:044f44184170 | 160 | FwdR = 0; |
dchase | 0:044f44184170 | 161 | } |
dchase | 0:044f44184170 | 162 | break; |
dchase | 0:044f44184170 | 163 | case '8': //button 8 right arrow |
dchase | 0:044f44184170 | 164 | if (bhit=='1') { |
dchase | 0:044f44184170 | 165 | SpeedL = 0.5; |
dchase | 0:044f44184170 | 166 | SpeedR = 0.5; |
dchase | 0:044f44184170 | 167 | FwdL = 1; |
dchase | 0:044f44184170 | 168 | BckR = 1; |
dchase | 0:044f44184170 | 169 | } else { |
dchase | 0:044f44184170 | 170 | for (p = SpeedL; p > 0; p -= 0.1) { |
dchase | 0:044f44184170 | 171 | SpeedL = p; |
dchase | 0:044f44184170 | 172 | SpeedR = p; |
dchase | 0:044f44184170 | 173 | } |
dchase | 0:044f44184170 | 174 | FwdL = 0; |
dchase | 0:044f44184170 | 175 | BckR = 0; |
dchase | 0:044f44184170 | 176 | } |
dchase | 0:044f44184170 | 177 | } |
dchase | 0:044f44184170 | 178 | } |
dchase | 0:044f44184170 | 179 | } |
dchase | 0:044f44184170 | 180 | } |
dchase | 0:044f44184170 | 181 | } // end readable |
dchase | 0:044f44184170 | 182 | } // end else |
dchase | 0:044f44184170 | 183 | Thread::wait(100); |
dchase | 0:044f44184170 | 184 | } //end while |
dchase | 0:044f44184170 | 185 | } // end Motor |
dchase | 0:044f44184170 | 186 | |
dchase | 0:044f44184170 | 187 | int main() |
dchase | 0:044f44184170 | 188 | { |
dchase | 0:044f44184170 | 189 | DistFlag = 0; |
dchase | 0:044f44184170 | 190 | for (p = 0; p < 1; p += 0.1) { |
dchase | 0:044f44184170 | 191 | ledB = p; |
dchase | 0:044f44184170 | 192 | wait(0.2); |
dchase | 0:044f44184170 | 193 | } |
dchase | 0:044f44184170 | 194 | wait(0.05); |
dchase | 0:044f44184170 | 195 | ledB = 0; |
dchase | 0:044f44184170 | 196 | t1.start(sensor); |
dchase | 0:044f44184170 | 197 | t2.start(motor); |
dchase | 0:044f44184170 | 198 | while (1) { |
dchase | 0:044f44184170 | 199 | if (DistFlag == 1) { |
dchase | 0:044f44184170 | 200 | while (distance <= 400) { |
dchase | 0:044f44184170 | 201 | SpeedL = 0.2; |
dchase | 0:044f44184170 | 202 | SpeedR = 0.2; |
dchase | 0:044f44184170 | 203 | BckL = 1; |
dchase | 0:044f44184170 | 204 | BckR = 1; |
dchase | 0:044f44184170 | 205 | Thread::wait(10); //Reupdate Distance |
dchase | 0:044f44184170 | 206 | } |
dchase | 0:044f44184170 | 207 | BckL = 0; |
dchase | 0:044f44184170 | 208 | BckR = 0; |
dchase | 0:044f44184170 | 209 | ledR = 0; |
dchase | 0:044f44184170 | 210 | DistFlag = 0; |
dchase | 0:044f44184170 | 211 | } |
dchase | 0:044f44184170 | 212 | Thread::wait(500); |
dchase | 0:044f44184170 | 213 | } |
dchase | 0:044f44184170 | 214 | } |