Jered Tupik / Mbed 2 deprecated Multiple_Scanning_LiDAR_Bot

This code allows for an Mbed LPC1768 to be operated over the Adafruit Bluetooth controller app. Directional controls send serial commands over Bluetooth to control the motors over using an H-Bridge, and with four directional LiDAR sensors and a speaker/uLCD providing for collision warning. The user may also activate autonomous mode, in which case the code will begin scanning the LiDARs and avoiding obstacles without user intervention.

Dependencies:   mbed Servo mbed-rtos 4DGL-uLCD-SE Motor VL53L0X

main.cpp@0:831e390ffcf8, 2019-12-04 (annotated)

Committer:
HighTide
Date:
Wed Dec 04 15:44:11 2019 +0000
Revision:
0:831e390ffcf8
Final version of Multiple Scanning LiDAR Bot for ECE 4180.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
HighTide 0:831e390ffcf8 1 #include "mbed.h"
HighTide 0:831e390ffcf8 2 #include "rtos.h"
HighTide 0:831e390ffcf8 3 #include "Motor.h"
HighTide 0:831e390ffcf8 4 #include "Servo.h"
HighTide 0:831e390ffcf8 5 #include "VL53L0X.h"
HighTide 0:831e390ffcf8 6 #include "LSM9DS1.h"
HighTide 0:831e390ffcf8 7 #include "uLCD_4DGL.h"
HighTide 0:831e390ffcf8 8
HighTide 0:831e390ffcf8 9 #define IN_RANGE(x) ((x) > 0 && (x) < 300)
HighTide 0:831e390ffcf8 10 #define AUTO_SPEED 0.5
HighTide 0:831e390ffcf8 11
HighTide 0:831e390ffcf8 12 Serial pc(USBTX, USBRX);
HighTide 0:831e390ffcf8 13 Serial btc(p13, p14);
HighTide 0:831e390ffcf8 14 volatile bool user_mode = true;
HighTide 0:831e390ffcf8 15
HighTide 0:831e390ffcf8 16 DevI2C i2c(p9, p10);
HighTide 0:831e390ffcf8 17 DigitalOut shdn_left(p20), shdn_up(p17), shdn_right(p19), shdn_down(p18);
HighTide 0:831e390ffcf8 18 VL53L0X l_left(&i2c, &shdn_left, NC), l_up(&i2c, &shdn_up, NC), l_right(&i2c, &shdn_right, NC), l_down(&i2c, &shdn_down, NC);
HighTide 0:831e390ffcf8 19 PwmOut speaker(p25);
HighTide 0:831e390ffcf8 20 uLCD_4DGL uLCD(p28, p27, p15);
HighTide 0:831e390ffcf8 21 uint32_t distances[16];
HighTide 0:831e390ffcf8 22 volatile bool ob_left = false, ob_up = false, ob_right = false, ob_down = false;
HighTide 0:831e390ffcf8 23
HighTide 0:831e390ffcf8 24 Motor left(p21, p29, p30);
HighTide 0:831e390ffcf8 25 Motor right(p22, p23, p24);
HighTide 0:831e390ffcf8 26 Servo servo(p26);
HighTide 0:831e390ffcf8 27 LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);
HighTide 0:831e390ffcf8 28
HighTide 0:831e390ffcf8 29 DigitalOut led1(LED1), led2(LED2), led3(LED3), led4(LED4);
HighTide 0:831e390ffcf8 30
HighTide 0:831e390ffcf8 31 void read_btc(void const * args) {
HighTide 0:831e390ffcf8 32 char cmd[5] = {0}, num_chars = 0;
HighTide 0:831e390ffcf8 33 char pressed = 0;
HighTide 0:831e390ffcf8 34
HighTide 0:831e390ffcf8 35 while (1) {
HighTide 0:831e390ffcf8 36 num_chars = 0;
HighTide 0:831e390ffcf8 37 for (num_chars = 0; num_chars < 5; num_chars++) {
HighTide 0:831e390ffcf8 38 if (!btc.readable()) {
HighTide 0:831e390ffcf8 39 break;
HighTide 0:831e390ffcf8 40 }
HighTide 0:831e390ffcf8 41 cmd[num_chars] = btc.getc();
HighTide 0:831e390ffcf8 42 }
HighTide 0:831e390ffcf8 43
HighTide 0:831e390ffcf8 44 if (num_chars != 5) {
HighTide 0:831e390ffcf8 45 if (user_mode) {
HighTide 0:831e390ffcf8 46 left.speed(0.0);
HighTide 0:831e390ffcf8 47 right.speed(0.0);
HighTide 0:831e390ffcf8 48 }
HighTide 0:831e390ffcf8 49 } else if (cmd[0] == '!') {
HighTide 0:831e390ffcf8 50 if (cmd[1] == 'B') {
HighTide 0:831e390ffcf8 51 if (cmd[3] == '0') {
HighTide 0:831e390ffcf8 52 pressed &= ~(0x1 << (cmd[2] - '1'));
HighTide 0:831e390ffcf8 53 } else {
HighTide 0:831e390ffcf8 54 switch(cmd[2]) {
HighTide 0:831e390ffcf8 55 case '1': // Button 1 - Switch Running Mode
HighTide 0:831e390ffcf8 56 left.speed(0.0);
HighTide 0:831e390ffcf8 57 right.speed(0.0);
HighTide 0:831e390ffcf8 58 user_mode = !user_mode;
HighTide 0:831e390ffcf8 59 break;
HighTide 0:831e390ffcf8 60 case '4': // Emergency Reset
HighTide 0:831e390ffcf8 61 left.speed(0.0);
HighTide 0:831e390ffcf8 62 right.speed(0.0);
HighTide 0:831e390ffcf8 63 pressed = 0;
HighTide 0:831e390ffcf8 64 user_mode = true;
HighTide 0:831e390ffcf8 65 break;
HighTide 0:831e390ffcf8 66 case '5': // Up
HighTide 0:831e390ffcf8 67 case '6': // Down
HighTide 0:831e390ffcf8 68 case '7': // Left
HighTide 0:831e390ffcf8 69 case '8': // Right
HighTide 0:831e390ffcf8 70 pressed |= (0x1 << (cmd[2] - '1'));
HighTide 0:831e390ffcf8 71 break;
HighTide 0:831e390ffcf8 72 default:
HighTide 0:831e390ffcf8 73 left.speed(0.0);
HighTide 0:831e390ffcf8 74 right.speed(0.0);
HighTide 0:831e390ffcf8 75 break;
HighTide 0:831e390ffcf8 76 }
HighTide 0:831e390ffcf8 77 }
HighTide 0:831e390ffcf8 78 }
HighTide 0:831e390ffcf8 79 }
HighTide 0:831e390ffcf8 80
HighTide 0:831e390ffcf8 81 if (user_mode) {
HighTide 0:831e390ffcf8 82 switch (pressed >> 4) {
HighTide 0:831e390ffcf8 83 case 0b0000:
HighTide 0:831e390ffcf8 84 case 0b0011:
HighTide 0:831e390ffcf8 85 case 0b1100:
HighTide 0:831e390ffcf8 86 case 0b1111:
HighTide 0:831e390ffcf8 87 left.speed(0.0);
HighTide 0:831e390ffcf8 88 right.speed(0.0);
HighTide 0:831e390ffcf8 89 break;
HighTide 0:831e390ffcf8 90 case 0b0001:
HighTide 0:831e390ffcf8 91 case 0b1101:
HighTide 0:831e390ffcf8 92 left.speed(1.0);
HighTide 0:831e390ffcf8 93 right.speed(1.0);
HighTide 0:831e390ffcf8 94 break;
HighTide 0:831e390ffcf8 95 case 0b0010:
HighTide 0:831e390ffcf8 96 case 0b1110:
HighTide 0:831e390ffcf8 97 left.speed(-1.0);
HighTide 0:831e390ffcf8 98 right.speed(-1.0);
HighTide 0:831e390ffcf8 99 break;
HighTide 0:831e390ffcf8 100 case 0b0100:
HighTide 0:831e390ffcf8 101 case 0b0111:
HighTide 0:831e390ffcf8 102 left.speed(1.0);
HighTide 0:831e390ffcf8 103 right.speed(-1.0);
HighTide 0:831e390ffcf8 104 break;
HighTide 0:831e390ffcf8 105 case 0b0101:
HighTide 0:831e390ffcf8 106 left.speed(0.5);
HighTide 0:831e390ffcf8 107 right.speed(1.0);
HighTide 0:831e390ffcf8 108 break;
HighTide 0:831e390ffcf8 109 case 0b0110:
HighTide 0:831e390ffcf8 110 left.speed(-0.5);
HighTide 0:831e390ffcf8 111 right.speed(-1.0);
HighTide 0:831e390ffcf8 112 break;
HighTide 0:831e390ffcf8 113 case 0b1000:
HighTide 0:831e390ffcf8 114 case 0b1011:
HighTide 0:831e390ffcf8 115 left.speed(-1.0);
HighTide 0:831e390ffcf8 116 right.speed(1.0);
HighTide 0:831e390ffcf8 117 break;
HighTide 0:831e390ffcf8 118 case 0b1001:
HighTide 0:831e390ffcf8 119 left.speed(1.0);
HighTide 0:831e390ffcf8 120 right.speed(0.5);
HighTide 0:831e390ffcf8 121 break;
HighTide 0:831e390ffcf8 122 case 0b1010:
HighTide 0:831e390ffcf8 123 left.speed(-1.0);
HighTide 0:831e390ffcf8 124 right.speed(-0.5);
HighTide 0:831e390ffcf8 125 break;
HighTide 0:831e390ffcf8 126 default:
HighTide 0:831e390ffcf8 127 break;
HighTide 0:831e390ffcf8 128 }
HighTide 0:831e390ffcf8 129 }
HighTide 0:831e390ffcf8 130 led1 = !led1;
HighTide 0:831e390ffcf8 131 Thread::wait(100);
HighTide 0:831e390ffcf8 132 }
HighTide 0:831e390ffcf8 133 }
HighTide 0:831e390ffcf8 134
HighTide 0:831e390ffcf8 135 void scan_lidar(void const*args) {
HighTide 0:831e390ffcf8 136 while(1) {
HighTide 0:831e390ffcf8 137 if (user_mode) {
HighTide 0:831e390ffcf8 138 servo = 0.0;
HighTide 0:831e390ffcf8 139 l_left.get_distance(&distances[0]);
HighTide 0:831e390ffcf8 140 l_up.get_distance(&distances[4]);
HighTide 0:831e390ffcf8 141 l_right.get_distance(&distances[8]);
HighTide 0:831e390ffcf8 142 l_down.get_distance(&distances[12]);
HighTide 0:831e390ffcf8 143 } else {
HighTide 0:831e390ffcf8 144 for (int i = 0; i < 4; i++) {
HighTide 0:831e390ffcf8 145 servo = i * 0.125;
HighTide 0:831e390ffcf8 146 l_left.get_distance(&distances[i]);
HighTide 0:831e390ffcf8 147 l_up.get_distance(&distances[4+i]);
HighTide 0:831e390ffcf8 148 l_right.get_distance(&distances[8+i]);
HighTide 0:831e390ffcf8 149 l_down.get_distance(&distances[12+i]);
HighTide 0:831e390ffcf8 150 }
HighTide 0:831e390ffcf8 151 servo = 0.0;
HighTide 0:831e390ffcf8 152 }
HighTide 0:831e390ffcf8 153 led2 = !led2;
HighTide 0:831e390ffcf8 154 Thread::wait(100);
HighTide 0:831e390ffcf8 155 }
HighTide 0:831e390ffcf8 156 }
HighTide 0:831e390ffcf8 157
HighTide 0:831e390ffcf8 158 void collision_warning(void const* args) {
HighTide 0:831e390ffcf8 159
HighTide 0:831e390ffcf8 160 while(1) {
HighTide 0:831e390ffcf8 161 if (user_mode) {
HighTide 0:831e390ffcf8 162 ob_left = IN_RANGE(distances[0]);
HighTide 0:831e390ffcf8 163 ob_up = IN_RANGE(distances[4]);
HighTide 0:831e390ffcf8 164 ob_right = IN_RANGE(distances[8]);
HighTide 0:831e390ffcf8 165 ob_down = IN_RANGE(distances[12]);
HighTide 0:831e390ffcf8 166 } else {
HighTide 0:831e390ffcf8 167 ob_left = ob_up = ob_right = ob_down = false;
HighTide 0:831e390ffcf8 168 for (int i = 0; i < 4; i++) {
HighTide 0:831e390ffcf8 169 if (IN_RANGE(distances[(15+i)%16])) ob_left = true;
HighTide 0:831e390ffcf8 170 if (IN_RANGE(distances[3+i])) ob_up = true;
HighTide 0:831e390ffcf8 171 if (IN_RANGE(distances[7+i])) ob_right = true;
HighTide 0:831e390ffcf8 172 if (IN_RANGE(distances[11+i])) ob_down = true;
HighTide 0:831e390ffcf8 173 }
HighTide 0:831e390ffcf8 174 }
HighTide 0:831e390ffcf8 175
HighTide 0:831e390ffcf8 176 if (ob_left || ob_up || ob_right || ob_down) {
HighTide 0:831e390ffcf8 177 speaker = 0.5;
HighTide 0:831e390ffcf8 178 } else {
HighTide 0:831e390ffcf8 179 speaker = 0.0;
HighTide 0:831e390ffcf8 180 }
HighTide 0:831e390ffcf8 181 uLCD.filled_rectangle(0, 0, 32, 128, ob_left ? RED : BLACK);
HighTide 0:831e390ffcf8 182 uLCD.filled_rectangle(0, 0, 128, 32, ob_up ? RED : BLACK);
HighTide 0:831e390ffcf8 183 uLCD.filled_rectangle(96, 0, 128, 128, ob_right ? RED : BLACK);
HighTide 0:831e390ffcf8 184 uLCD.filled_rectangle(0, 96, 128, 128, ob_down ? RED : BLACK);
HighTide 0:831e390ffcf8 185 led3 = !led3;
HighTide 0:831e390ffcf8 186 Thread::wait(100);
HighTide 0:831e390ffcf8 187 }
HighTide 0:831e390ffcf8 188 }
HighTide 0:831e390ffcf8 189
HighTide 0:831e390ffcf8 190 void heartbeat(void const* args) {
HighTide 0:831e390ffcf8 191 while(1) {
HighTide 0:831e390ffcf8 192 led4 = !led4;
HighTide 0:831e390ffcf8 193 Thread::wait(1000);
HighTide 0:831e390ffcf8 194 }
HighTide 0:831e390ffcf8 195 }
HighTide 0:831e390ffcf8 196
HighTide 0:831e390ffcf8 197 int main() {
HighTide 0:831e390ffcf8 198
HighTide 0:831e390ffcf8 199 led1 = led2 = led3 = led4 = 0;
HighTide 0:831e390ffcf8 200 servo.calibrate(0.001, 45.0);
HighTide 0:831e390ffcf8 201 servo = 0.0;
HighTide 0:831e390ffcf8 202 left.speed(0.0);
HighTide 0:831e390ffcf8 203 right.speed(0.0);
HighTide 0:831e390ffcf8 204 uLCD.baudrate(3000000);
HighTide 0:831e390ffcf8 205 uLCD.cls();
HighTide 0:831e390ffcf8 206 uLCD.filled_rectangle(32, 32, 96, 96, BLUE);
HighTide 0:831e390ffcf8 207 speaker = 0.0;
HighTide 0:831e390ffcf8 208
HighTide 0:831e390ffcf8 209 l_left.VL53L0X_off();
HighTide 0:831e390ffcf8 210 l_up.VL53L0X_off();
HighTide 0:831e390ffcf8 211 l_right.VL53L0X_off();
HighTide 0:831e390ffcf8 212 l_down.VL53L0X_off();
HighTide 0:831e390ffcf8 213 l_left.init_sensor(0x30);
HighTide 0:831e390ffcf8 214 l_up.init_sensor(0x32);
HighTide 0:831e390ffcf8 215 l_right.init_sensor(0x34);
HighTide 0:831e390ffcf8 216 l_down.init_sensor(0x36);
HighTide 0:831e390ffcf8 217
HighTide 0:831e390ffcf8 218 Thread t1(read_btc);
HighTide 0:831e390ffcf8 219 Thread t2(scan_lidar);
HighTide 0:831e390ffcf8 220 Thread t3(collision_warning);
HighTide 0:831e390ffcf8 221 Thread t4(heartbeat);
HighTide 0:831e390ffcf8 222
HighTide 0:831e390ffcf8 223 while(1) {
HighTide 0:831e390ffcf8 224 if (!user_mode) {
HighTide 0:831e390ffcf8 225 if (ob_up) {
HighTide 0:831e390ffcf8 226 if (ob_left && ob_right) {
HighTide 0:831e390ffcf8 227 left.speed(-AUTO_SPEED);
HighTide 0:831e390ffcf8 228 right.speed(AUTO_SPEED);
HighTide 0:831e390ffcf8 229 wait(1.0);
HighTide 0:831e390ffcf8 230 left.speed(0.0);
HighTide 0:831e390ffcf8 231 right.speed(0.0);
HighTide 0:831e390ffcf8 232 } else if (ob_right) {
HighTide 0:831e390ffcf8 233 left.speed(AUTO_SPEED);
HighTide 0:831e390ffcf8 234 right.speed(-AUTO_SPEED);
HighTide 0:831e390ffcf8 235 } else {
HighTide 0:831e390ffcf8 236 left.speed(-AUTO_SPEED);
HighTide 0:831e390ffcf8 237 right.speed(AUTO_SPEED);
HighTide 0:831e390ffcf8 238 }
HighTide 0:831e390ffcf8 239 } else if (ob_left && ob_right) {
HighTide 0:831e390ffcf8 240 left.speed(AUTO_SPEED);
HighTide 0:831e390ffcf8 241 right.speed(AUTO_SPEED);
HighTide 0:831e390ffcf8 242 } else if (ob_left) {
HighTide 0:831e390ffcf8 243 left.speed(AUTO_SPEED);
HighTide 0:831e390ffcf8 244 right.speed(AUTO_SPEED / 2);
HighTide 0:831e390ffcf8 245 } else if (ob_right) {
HighTide 0:831e390ffcf8 246 left.speed(AUTO_SPEED / 2);
HighTide 0:831e390ffcf8 247 right.speed(AUTO_SPEED);
HighTide 0:831e390ffcf8 248 } else {
HighTide 0:831e390ffcf8 249 left.speed(AUTO_SPEED);
HighTide 0:831e390ffcf8 250 right.speed(AUTO_SPEED);
HighTide 0:831e390ffcf8 251 }
HighTide 0:831e390ffcf8 252 }
HighTide 0:831e390ffcf8 253 Thread::yield();
HighTide 0:831e390ffcf8 254 }
HighTide 0:831e390ffcf8 255 }