Daniel Chase
Bluetooth controlled Robot with Smart Collision Detection for ECE 4180 Final Project
Dependencies: Servo X_NUCLEO_53L0A1 mbed-rtos mbed
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 #include "XNucleo53L0A1.h" //Lidar Distance Sensor: 3.3V VIN, GND GND, P13 SDA, P14 SCL, P26 SHDN 00003 #include "Servo.h" 00004 #include "rtos.h" 00005 #include <stdio.h> 00006 00007 BusOut myled(LED1,LED2,LED3,LED4); 00008 Serial blue(p13,p14); //bluetooth UART 00009 00010 Servo myservo(p23); //Servo connected to PWM 23 00011 00012 float p; 00013 PwmOut ledR(p26); //RGB Red 00014 PwmOut ledG(p25); //RGB Green 00015 PwmOut ledB(p24); //RGB Blue 00016 00017 PwmOut SpeedL(p21); //Speed of Left motor 00018 PwmOut SpeedR(p22); //Speed of Right motor 00019 DigitalOut FwdL(p15); //Forward Left motor 00020 DigitalOut FwdR(p16); //Forward Right motor 00021 DigitalOut BckL(p19); //Backward Left motor 00022 DigitalOut BckR(p18); //Backward Right motor 00023 00024 /* I2C LIDAR sensor */ 00025 #define VL53L0_I2C_SDA p28 00026 #define VL53L0_I2C_SCL p27 00027 DevI2C *device_i2c = new DevI2C(VL53L0_I2C_SDA, VL53L0_I2C_SCL); 00028 static XNucleo53L0A1 *board = XNucleo53L0A1::instance(device_i2c, A2, D8, D2);; // LIDAR singleton obj instantiate 00029 DigitalOut shdn(p29); 00030 int status; 00031 uint32_t sensorData; 00032 volatile int distance; 00033 int DistFlag; //used to determine if collision would have occured 00034 00035 Thread t1; 00036 Thread t2; 00037 00038 void sensor() 00039 { 00040 shdn = 0; //must reset sensor for an mbed reset to work 00041 wait(0.1); 00042 shdn = 1; 00043 wait(0.1); 00044 // init the 53L0A1 board with default values 00045 status = board->init_board(); 00046 while (status) { 00047 pc.printf("Failed to init board! \r\n"); 00048 status = board->init_board(); 00049 } 00050 pc.printf("Board Init. Beginning Loop \r\n"); 00051 //loop taking and printing distance 00052 while (1) { 00053 status = board->sensor_centre->get_distance(&sensorData); 00054 distance = sensorData; 00055 if (status == VL53L0X_ERROR_NONE) { 00056 if (distance <= 400) DistFlag = 1; 00057 pc.printf("D=%ld mm // DistFlag = %d\r\n", distance, DistFlag); 00058 } 00059 Thread::wait(10); 00060 }//end while 00061 } // end sensor 00062 00063 void motor() 00064 { 00065 char bnum=0; 00066 char bhit=0; 00067 while (1) { 00068 if (DistFlag == 1) { 00069 ledG = 0; 00070 ledR = 1; 00071 SpeedL = 0.5; 00072 SpeedR = 0.5; 00073 FwdL = 0; 00074 FwdR = 0; 00075 } else { 00076 ledG = 1; 00077 if (blue.readable()) { 00078 if (blue.getc()=='!') { 00079 if (blue.getc()=='B') { //button data packet 00080 bnum = blue.getc(); //button number 00081 bhit = blue.getc(); //1=hit, 0=release 00082 if (blue.getc()==char(~('!' + 'B' + bnum + bhit))) { //checksum OK? 00083 myled = bnum - '0'; //current button number will appear on LEDs 00084 switch (bnum) { 00085 case '1': //number button 1: Servo Sweep 00086 if (bhit=='1') { 00087 for(float p=0; p<1.0; p += 0.1) { 00088 myservo = p; 00089 wait(0.2); 00090 } 00091 } else { 00092 myservo = 0.5; 00093 } 00094 break; 00095 case '2': //number button 2 00096 if (bhit=='1') { 00097 //add hit code here 00098 DistFlag = 0; 00099 } else { 00100 //add release code here 00101 } 00102 break; 00103 case '3': //number button 3 00104 if (bhit=='1') { 00105 //add hit code here 00106 } else { 00107 //add release code here 00108 } 00109 break; 00110 case '4': //number button 4 00111 if (bhit=='1') { 00112 //add hit code here 00113 } else { 00114 //add release code here 00115 } 00116 break; 00117 case '5': //button 5 up arrow 00118 //pc.printf("Pressed F\n"); 00119 if (bhit=='1') { 00120 SpeedL = 1; 00121 SpeedR = 1; 00122 FwdL = 1; 00123 FwdR = 1; 00124 } else { 00125 for (p = SpeedL; p > 0; p -= 0.1) { 00126 SpeedL = p; 00127 SpeedR = p; 00128 } 00129 FwdL = 0; 00130 FwdR = 0; 00131 } 00132 break; 00133 case '6': //button 6 down arrow 00134 if (bhit=='1') { 00135 SpeedL = 0.5; 00136 SpeedR = 0.5; 00137 BckL = 1; 00138 BckR = 1; 00139 } else { 00140 for (p = SpeedL; p > 0; p -= 0.1) { 00141 SpeedL = p; 00142 SpeedR = p; 00143 } 00144 BckL = 0; 00145 BckR = 0; 00146 } 00147 break; 00148 case '7': //button 7 left arrow 00149 if (bhit=='1') { 00150 SpeedL = 0.5; 00151 SpeedR = 0.5; 00152 BckL = 1; 00153 FwdR = 1; 00154 } else { 00155 for (p = SpeedL; p > 0; p -= 0.1) { 00156 SpeedL = p; 00157 SpeedR = p; 00158 } 00159 BckL = 0; 00160 FwdR = 0; 00161 } 00162 break; 00163 case '8': //button 8 right arrow 00164 if (bhit=='1') { 00165 SpeedL = 0.5; 00166 SpeedR = 0.5; 00167 FwdL = 1; 00168 BckR = 1; 00169 } else { 00170 for (p = SpeedL; p > 0; p -= 0.1) { 00171 SpeedL = p; 00172 SpeedR = p; 00173 } 00174 FwdL = 0; 00175 BckR = 0; 00176 } 00177 } 00178 } 00179 } 00180 } 00181 } // end readable 00182 } // end else 00183 Thread::wait(100); 00184 } //end while 00185 } // end Motor 00186 00187 int main() 00188 { 00189 DistFlag = 0; 00190 for (p = 0; p < 1; p += 0.1) { 00191 ledB = p; 00192 wait(0.2); 00193 } 00194 wait(0.05); 00195 ledB = 0; 00196 t1.start(sensor); 00197 t2.start(motor); 00198 while (1) { 00199 if (DistFlag == 1) { 00200 while (distance <= 400) { 00201 SpeedL = 0.2; 00202 SpeedR = 0.2; 00203 BckL = 1; 00204 BckR = 1; 00205 Thread::wait(10); //Reupdate Distance 00206 } 00207 BckL = 0; 00208 BckR = 0; 00209 ledR = 0; 00210 DistFlag = 0; 00211 } 00212 Thread::wait(500); 00213 } 00214 }
Generated on Sat Jul 16 2022 17:04:30 by
1.7.2