Dependencies: mbed Servo mbed-rtos 4DGL-uLCD-SE Motor VL53L0X
Diff: main.cpp
- Revision:
- 0:831e390ffcf8
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed Dec 04 15:44:11 2019 +0000
@@ -0,0 +1,255 @@
+#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();
+ }
+}
\ No newline at end of file