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
- Committer:
- HighTide
- Date:
- 2019-12-04
- Revision:
- 0:831e390ffcf8
File content as of revision 0:831e390ffcf8:
#include "mbed.h"
#include "rtos.h"
#include "Motor.h"
#include "Servo.h"
#include "VL53L0X.h"
#include "LSM9DS1.h"
#include "uLCD_4DGL.h"
#define IN_RANGE(x) ((x) > 0 && (x) < 300)
#define AUTO_SPEED 0.5
Serial pc(USBTX, USBRX);
Serial btc(p13, p14);
volatile bool user_mode = true;
DevI2C i2c(p9, p10);
DigitalOut shdn_left(p20), shdn_up(p17), shdn_right(p19), shdn_down(p18);
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);
PwmOut speaker(p25);
uLCD_4DGL uLCD(p28, p27, p15);
uint32_t distances[16];
volatile bool ob_left = false, ob_up = false, ob_right = false, ob_down = false;
Motor left(p21, p29, p30);
Motor right(p22, p23, p24);
Servo servo(p26);
LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);
DigitalOut led1(LED1), led2(LED2), led3(LED3), led4(LED4);
void read_btc(void const * args) {
char cmd[5] = {0}, num_chars = 0;
char pressed = 0;
while (1) {
num_chars = 0;
for (num_chars = 0; num_chars < 5; num_chars++) {
if (!btc.readable()) {
break;
}
cmd[num_chars] = btc.getc();
}
if (num_chars != 5) {
if (user_mode) {
left.speed(0.0);
right.speed(0.0);
}
} else if (cmd[0] == '!') {
if (cmd[1] == 'B') {
if (cmd[3] == '0') {
pressed &= ~(0x1 << (cmd[2] - '1'));
} else {
switch(cmd[2]) {
case '1': // Button 1 - Switch Running Mode
left.speed(0.0);
right.speed(0.0);
user_mode = !user_mode;
break;
case '4': // Emergency Reset
left.speed(0.0);
right.speed(0.0);
pressed = 0;
user_mode = true;
break;
case '5': // Up
case '6': // Down
case '7': // Left
case '8': // Right
pressed |= (0x1 << (cmd[2] - '1'));
break;
default:
left.speed(0.0);
right.speed(0.0);
break;
}
}
}
}
if (user_mode) {
switch (pressed >> 4) {
case 0b0000:
case 0b0011:
case 0b1100:
case 0b1111:
left.speed(0.0);
right.speed(0.0);
break;
case 0b0001:
case 0b1101:
left.speed(1.0);
right.speed(1.0);
break;
case 0b0010:
case 0b1110:
left.speed(-1.0);
right.speed(-1.0);
break;
case 0b0100:
case 0b0111:
left.speed(1.0);
right.speed(-1.0);
break;
case 0b0101:
left.speed(0.5);
right.speed(1.0);
break;
case 0b0110:
left.speed(-0.5);
right.speed(-1.0);
break;
case 0b1000:
case 0b1011:
left.speed(-1.0);
right.speed(1.0);
break;
case 0b1001:
left.speed(1.0);
right.speed(0.5);
break;
case 0b1010:
left.speed(-1.0);
right.speed(-0.5);
break;
default:
break;
}
}
led1 = !led1;
Thread::wait(100);
}
}
void scan_lidar(void const*args) {
while(1) {
if (user_mode) {
servo = 0.0;
l_left.get_distance(&distances[0]);
l_up.get_distance(&distances[4]);
l_right.get_distance(&distances[8]);
l_down.get_distance(&distances[12]);
} else {
for (int i = 0; i < 4; i++) {
servo = i * 0.125;
l_left.get_distance(&distances[i]);
l_up.get_distance(&distances[4+i]);
l_right.get_distance(&distances[8+i]);
l_down.get_distance(&distances[12+i]);
}
servo = 0.0;
}
led2 = !led2;
Thread::wait(100);
}
}
void collision_warning(void const* args) {
while(1) {
if (user_mode) {
ob_left = IN_RANGE(distances[0]);
ob_up = IN_RANGE(distances[4]);
ob_right = IN_RANGE(distances[8]);
ob_down = IN_RANGE(distances[12]);
} else {
ob_left = ob_up = ob_right = ob_down = false;
for (int i = 0; i < 4; i++) {
if (IN_RANGE(distances[(15+i)%16])) ob_left = true;
if (IN_RANGE(distances[3+i])) ob_up = true;
if (IN_RANGE(distances[7+i])) ob_right = true;
if (IN_RANGE(distances[11+i])) ob_down = true;
}
}
if (ob_left || ob_up || ob_right || ob_down) {
speaker = 0.5;
} else {
speaker = 0.0;
}
uLCD.filled_rectangle(0, 0, 32, 128, ob_left ? RED : BLACK);
uLCD.filled_rectangle(0, 0, 128, 32, ob_up ? RED : BLACK);
uLCD.filled_rectangle(96, 0, 128, 128, ob_right ? RED : BLACK);
uLCD.filled_rectangle(0, 96, 128, 128, ob_down ? RED : BLACK);
led3 = !led3;
Thread::wait(100);
}
}
void heartbeat(void const* args) {
while(1) {
led4 = !led4;
Thread::wait(1000);
}
}
int main() {
led1 = led2 = led3 = led4 = 0;
servo.calibrate(0.001, 45.0);
servo = 0.0;
left.speed(0.0);
right.speed(0.0);
uLCD.baudrate(3000000);
uLCD.cls();
uLCD.filled_rectangle(32, 32, 96, 96, BLUE);
speaker = 0.0;
l_left.VL53L0X_off();
l_up.VL53L0X_off();
l_right.VL53L0X_off();
l_down.VL53L0X_off();
l_left.init_sensor(0x30);
l_up.init_sensor(0x32);
l_right.init_sensor(0x34);
l_down.init_sensor(0x36);
Thread t1(read_btc);
Thread t2(scan_lidar);
Thread t3(collision_warning);
Thread t4(heartbeat);
while(1) {
if (!user_mode) {
if (ob_up) {
if (ob_left && ob_right) {
left.speed(-AUTO_SPEED);
right.speed(AUTO_SPEED);
wait(1.0);
left.speed(0.0);
right.speed(0.0);
} else if (ob_right) {
left.speed(AUTO_SPEED);
right.speed(-AUTO_SPEED);
} else {
left.speed(-AUTO_SPEED);
right.speed(AUTO_SPEED);
}
} else if (ob_left && ob_right) {
left.speed(AUTO_SPEED);
right.speed(AUTO_SPEED);
} else if (ob_left) {
left.speed(AUTO_SPEED);
right.speed(AUTO_SPEED / 2);
} else if (ob_right) {
left.speed(AUTO_SPEED / 2);
right.speed(AUTO_SPEED);
} else {
left.speed(AUTO_SPEED);
right.speed(AUTO_SPEED);
}
}
Thread::yield();
}
}