Bluetooth controlled Robot with Smart Collision Detection for ECE 4180 Final Project
Dependencies: Servo X_NUCLEO_53L0A1 mbed-rtos mbed
main.cpp
- Committer:
- dchase
- Date:
- 2018-04-27
- Revision:
- 0:044f44184170
File content as of revision 0:044f44184170:
#include "mbed.h" #include "XNucleo53L0A1.h" //Lidar Distance Sensor: 3.3V VIN, GND GND, P13 SDA, P14 SCL, P26 SHDN #include "Servo.h" #include "rtos.h" #include <stdio.h> BusOut myled(LED1,LED2,LED3,LED4); Serial blue(p13,p14); //bluetooth UART Servo myservo(p23); //Servo connected to PWM 23 float p; PwmOut ledR(p26); //RGB Red PwmOut ledG(p25); //RGB Green PwmOut ledB(p24); //RGB Blue PwmOut SpeedL(p21); //Speed of Left motor PwmOut SpeedR(p22); //Speed of Right motor DigitalOut FwdL(p15); //Forward Left motor DigitalOut FwdR(p16); //Forward Right motor DigitalOut BckL(p19); //Backward Left motor DigitalOut BckR(p18); //Backward Right motor /* I2C LIDAR sensor */ #define VL53L0_I2C_SDA p28 #define VL53L0_I2C_SCL p27 DevI2C *device_i2c = new DevI2C(VL53L0_I2C_SDA, VL53L0_I2C_SCL); static XNucleo53L0A1 *board = XNucleo53L0A1::instance(device_i2c, A2, D8, D2);; // LIDAR singleton obj instantiate DigitalOut shdn(p29); int status; uint32_t sensorData; volatile int distance; int DistFlag; //used to determine if collision would have occured Thread t1; Thread t2; void sensor() { shdn = 0; //must reset sensor for an mbed reset to work wait(0.1); shdn = 1; wait(0.1); // init the 53L0A1 board with default values status = board->init_board(); while (status) { pc.printf("Failed to init board! \r\n"); status = board->init_board(); } pc.printf("Board Init. Beginning Loop \r\n"); //loop taking and printing distance while (1) { status = board->sensor_centre->get_distance(&sensorData); distance = sensorData; if (status == VL53L0X_ERROR_NONE) { if (distance <= 400) DistFlag = 1; pc.printf("D=%ld mm // DistFlag = %d\r\n", distance, DistFlag); } Thread::wait(10); }//end while } // end sensor void motor() { char bnum=0; char bhit=0; while (1) { if (DistFlag == 1) { ledG = 0; ledR = 1; SpeedL = 0.5; SpeedR = 0.5; FwdL = 0; FwdR = 0; } else { ledG = 1; if (blue.readable()) { if (blue.getc()=='!') { if (blue.getc()=='B') { //button data packet bnum = blue.getc(); //button number bhit = blue.getc(); //1=hit, 0=release if (blue.getc()==char(~('!' + 'B' + bnum + bhit))) { //checksum OK? myled = bnum - '0'; //current button number will appear on LEDs switch (bnum) { case '1': //number button 1: Servo Sweep if (bhit=='1') { for(float p=0; p<1.0; p += 0.1) { myservo = p; wait(0.2); } } else { myservo = 0.5; } break; case '2': //number button 2 if (bhit=='1') { //add hit code here DistFlag = 0; } else { //add release code here } break; case '3': //number button 3 if (bhit=='1') { //add hit code here } else { //add release code here } break; case '4': //number button 4 if (bhit=='1') { //add hit code here } else { //add release code here } break; case '5': //button 5 up arrow //pc.printf("Pressed F\n"); if (bhit=='1') { SpeedL = 1; SpeedR = 1; FwdL = 1; FwdR = 1; } else { for (p = SpeedL; p > 0; p -= 0.1) { SpeedL = p; SpeedR = p; } FwdL = 0; FwdR = 0; } break; case '6': //button 6 down arrow if (bhit=='1') { SpeedL = 0.5; SpeedR = 0.5; BckL = 1; BckR = 1; } else { for (p = SpeedL; p > 0; p -= 0.1) { SpeedL = p; SpeedR = p; } BckL = 0; BckR = 0; } break; case '7': //button 7 left arrow if (bhit=='1') { SpeedL = 0.5; SpeedR = 0.5; BckL = 1; FwdR = 1; } else { for (p = SpeedL; p > 0; p -= 0.1) { SpeedL = p; SpeedR = p; } BckL = 0; FwdR = 0; } break; case '8': //button 8 right arrow if (bhit=='1') { SpeedL = 0.5; SpeedR = 0.5; FwdL = 1; BckR = 1; } else { for (p = SpeedL; p > 0; p -= 0.1) { SpeedL = p; SpeedR = p; } FwdL = 0; BckR = 0; } } } } } } // end readable } // end else Thread::wait(100); } //end while } // end Motor int main() { DistFlag = 0; for (p = 0; p < 1; p += 0.1) { ledB = p; wait(0.2); } wait(0.05); ledB = 0; t1.start(sensor); t2.start(motor); while (1) { if (DistFlag == 1) { while (distance <= 400) { SpeedL = 0.2; SpeedR = 0.2; BckL = 1; BckR = 1; Thread::wait(10); //Reupdate Distance } BckL = 0; BckR = 0; ledR = 0; DistFlag = 0; } Thread::wait(500); } }