Daniel Chase / Mbed 2 deprecated Final_Project

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?

UserRevisionLine numberNew 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 }