Elisha Mang / Mbed 2 deprecated Campus_Safety_Bot

ECE 4180 Final Project - Georgia Institute of Technology by LZ and EM

Dependencies:   mbed Motor BNO055 SDFileSystem LSM9DS1_Library_cal HC_SR04_Ultrasonic_Library

main.cpp@0:2266b80b252e, 2019-12-03 (annotated)

Committer:
elisham11
Date:
Tue Dec 03 18:57:39 2019 +0000
Revision:
0:2266b80b252e
ECE 4180 Project; This project build a Bluetooth control bot with imu navigation. H-bridge is setup to control two PWM motor. Sonar distance sensor and photo transistor - light sensor are used to collect data. MicroSD card fiel system stores the data.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
elisham11 0:2266b80b252e 1 #include "mbed.h"
elisham11 0:2266b80b252e 2 #include "ultrasonic.h"
elisham11 0:2266b80b252e 3 #include "SDFileSystem.h"
elisham11 0:2266b80b252e 4 #include <string>
elisham11 0:2266b80b252e 5 #include "Motor.h"
elisham11 0:2266b80b252e 6 #include "LSM9DS1.h"
elisham11 0:2266b80b252e 7 #define PI 3.14159
elisham11 0:2266b80b252e 8 #define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA.
elisham11 0:2266b80b252e 9 Serial pc(USBTX, USBRX);
elisham11 0:2266b80b252e 10
elisham11 0:2266b80b252e 11 DigitalOut led1(LED1);
elisham11 0:2266b80b252e 12 DigitalOut led2(LED2);
elisham11 0:2266b80b252e 13 DigitalOut led3(LED3);
elisham11 0:2266b80b252e 14 DigitalOut led4(LED4);
elisham11 0:2266b80b252e 15
elisham11 0:2266b80b252e 16 //H-bridge
elisham11 0:2266b80b252e 17 Motor m_left(p21, p15, p16); // pwm,fwd,rev pwmA
elisham11 0:2266b80b252e 18 Motor m_right(p22, p19, p20); // pwm,fwd,rev
elisham11 0:2266b80b252e 19
elisham11 0:2266b80b252e 20 //Photo transistor
elisham11 0:2266b80b252e 21 AnalogIn photocell(p18);
elisham11 0:2266b80b252e 22
elisham11 0:2266b80b252e 23 //Timer
elisham11 0:2266b80b252e 24 Timer t; //t.start(), t.stop(), t.read()
elisham11 0:2266b80b252e 25
elisham11 0:2266b80b252e 26 //SD Card
elisham11 0:2266b80b252e 27 SDFileSystem sd(p5, p6, p7, p8, "sd");
elisham11 0:2266b80b252e 28
elisham11 0:2266b80b252e 29 RawSerial Bluetooth(p13,p14); // tx, rx
elisham11 0:2266b80b252e 30
elisham11 0:2266b80b252e 31 //Global variables for bluetooth control
elisham11 0:2266b80b252e 32 bool sonarOn = false; //Indicates when sonar is taking measurements
elisham11 0:2266b80b252e 33 char heading = '0'; //Indicates rough direction of the robot; 'f' = forward, 'l' = left, 'r' = right, 'b' = backwards, '0' = not moving
elisham11 0:2266b80b252e 34 bool running = true; //Keeps main while loop going
elisham11 0:2266b80b252e 35
elisham11 0:2266b80b252e 36
elisham11 0:2266b80b252e 37 //Code for interrupt routine for Bluetooth input
elisham11 0:2266b80b252e 38 enum statetype {start = 0, got_exclm, got_B, got_one, got_two, got_three, got_five, got_six, got_seven, got_eight, got_11, got_21, got_31, got_51, got_61, got_71, got_81};
elisham11 0:2266b80b252e 39 statetype state = start;
elisham11 0:2266b80b252e 40 //Interrupt routine to parse message with one new character per serial RX interrupt
elisham11 0:2266b80b252e 41 void parse_message()
elisham11 0:2266b80b252e 42 {
elisham11 0:2266b80b252e 43 switch (state)
elisham11 0:2266b80b252e 44 {
elisham11 0:2266b80b252e 45 case start:
elisham11 0:2266b80b252e 46 if (Bluetooth.getc()=='!')
elisham11 0:2266b80b252e 47 {
elisham11 0:2266b80b252e 48 //led2 = 1;
elisham11 0:2266b80b252e 49 state = got_exclm;
elisham11 0:2266b80b252e 50 }
elisham11 0:2266b80b252e 51 else state = start;
elisham11 0:2266b80b252e 52 break;
elisham11 0:2266b80b252e 53 case got_exclm:
elisham11 0:2266b80b252e 54
elisham11 0:2266b80b252e 55 if (Bluetooth.getc() == 'B')
elisham11 0:2266b80b252e 56 {
elisham11 0:2266b80b252e 57 state = got_B;
elisham11 0:2266b80b252e 58 }
elisham11 0:2266b80b252e 59 else state = start;
elisham11 0:2266b80b252e 60 break;
elisham11 0:2266b80b252e 61 case got_B:
elisham11 0:2266b80b252e 62 {
elisham11 0:2266b80b252e 63 char recv = Bluetooth.getc();
elisham11 0:2266b80b252e 64 if (recv == '1') state = got_one;
elisham11 0:2266b80b252e 65 else if (recv == '2') state = got_two;
elisham11 0:2266b80b252e 66 else if (recv == '3') state = got_three;
elisham11 0:2266b80b252e 67 else if (recv == '5') state = got_five;
elisham11 0:2266b80b252e 68 else if (recv == '6') state = got_six;
elisham11 0:2266b80b252e 69 else if (recv == '7') state = got_seven;
elisham11 0:2266b80b252e 70 else if (recv == '8') state = got_eight;
elisham11 0:2266b80b252e 71 else state = start;
elisham11 0:2266b80b252e 72 }
elisham11 0:2266b80b252e 73 break;
elisham11 0:2266b80b252e 74 case got_one:
elisham11 0:2266b80b252e 75 if (Bluetooth.getc() == '1')
elisham11 0:2266b80b252e 76 {
elisham11 0:2266b80b252e 77 //Make sure motors are stopped
elisham11 0:2266b80b252e 78 m_left.speed(0.0);
elisham11 0:2266b80b252e 79 m_right.speed(0.0);
elisham11 0:2266b80b252e 80 sonarOn = false;
elisham11 0:2266b80b252e 81 heading = '0';
elisham11 0:2266b80b252e 82 running = false;
elisham11 0:2266b80b252e 83 }
elisham11 0:2266b80b252e 84 else state = start;
elisham11 0:2266b80b252e 85 break;
elisham11 0:2266b80b252e 86 case got_two: //Stop everything
elisham11 0:2266b80b252e 87 if (Bluetooth.getc() == '1')
elisham11 0:2266b80b252e 88 {
elisham11 0:2266b80b252e 89
elisham11 0:2266b80b252e 90 }
elisham11 0:2266b80b252e 91 else state = start;
elisham11 0:2266b80b252e 92 break;
elisham11 0:2266b80b252e 93 case got_three: //currently unimplemented
elisham11 0:2266b80b252e 94 if (Bluetooth.getc() == '1')
elisham11 0:2266b80b252e 95 {
elisham11 0:2266b80b252e 96
elisham11 0:2266b80b252e 97 }
elisham11 0:2266b80b252e 98 else state = start;
elisham11 0:2266b80b252e 99 break;
elisham11 0:2266b80b252e 100 case got_five: //up arrow pressed
elisham11 0:2266b80b252e 101 {
elisham11 0:2266b80b252e 102 char recv = Bluetooth.getc();
elisham11 0:2266b80b252e 103 if (recv == '1') //Button pressed or held
elisham11 0:2266b80b252e 104 {
elisham11 0:2266b80b252e 105 sonarOn = true;
elisham11 0:2266b80b252e 106 heading = 'f';
elisham11 0:2266b80b252e 107 m_left.speed(0.42);
elisham11 0:2266b80b252e 108 m_right.speed(0.4);
elisham11 0:2266b80b252e 109 }
elisham11 0:2266b80b252e 110 else if (recv == '0') //Button released
elisham11 0:2266b80b252e 111 {
elisham11 0:2266b80b252e 112 //sonarOn = false;
elisham11 0:2266b80b252e 113 heading = '0';
elisham11 0:2266b80b252e 114 m_left.speed(0.0);
elisham11 0:2266b80b252e 115 m_right.speed(0.0);
elisham11 0:2266b80b252e 116 }
elisham11 0:2266b80b252e 117 else state = start;
elisham11 0:2266b80b252e 118 }
elisham11 0:2266b80b252e 119 break;
elisham11 0:2266b80b252e 120 case got_six: //down arrow pressed
elisham11 0:2266b80b252e 121 {
elisham11 0:2266b80b252e 122 char recv = Bluetooth.getc();
elisham11 0:2266b80b252e 123 if (recv == '1') //Button pressed or held
elisham11 0:2266b80b252e 124 {
elisham11 0:2266b80b252e 125 sonarOn = true;
elisham11 0:2266b80b252e 126 heading = 'b';
elisham11 0:2266b80b252e 127 m_left.speed(-0.42);
elisham11 0:2266b80b252e 128 m_right.speed(-0.4);
elisham11 0:2266b80b252e 129 }
elisham11 0:2266b80b252e 130 else if (recv == '0')
elisham11 0:2266b80b252e 131 {
elisham11 0:2266b80b252e 132 //sonarOn = false;
elisham11 0:2266b80b252e 133 heading = '0';
elisham11 0:2266b80b252e 134 m_left.speed(0.0);
elisham11 0:2266b80b252e 135 m_right.speed(0.0);
elisham11 0:2266b80b252e 136 }
elisham11 0:2266b80b252e 137 else state = start;
elisham11 0:2266b80b252e 138 }
elisham11 0:2266b80b252e 139 break;
elisham11 0:2266b80b252e 140 case got_seven: //turn left
elisham11 0:2266b80b252e 141 {
elisham11 0:2266b80b252e 142 char recv = Bluetooth.getc();
elisham11 0:2266b80b252e 143 //sonarOn = false;
elisham11 0:2266b80b252e 144 if (recv == '1')
elisham11 0:2266b80b252e 145 {
elisham11 0:2266b80b252e 146 heading = 'l';
elisham11 0:2266b80b252e 147 m_left.speed(-0.3);
elisham11 0:2266b80b252e 148 m_right.speed(0.3);
elisham11 0:2266b80b252e 149 }
elisham11 0:2266b80b252e 150 else if (recv == '0')
elisham11 0:2266b80b252e 151 {
elisham11 0:2266b80b252e 152 heading = '0';
elisham11 0:2266b80b252e 153 m_left.speed(0.0);
elisham11 0:2266b80b252e 154 m_right.speed(0.0);
elisham11 0:2266b80b252e 155 }
elisham11 0:2266b80b252e 156 else state = start;
elisham11 0:2266b80b252e 157 }
elisham11 0:2266b80b252e 158 break;
elisham11 0:2266b80b252e 159 case got_eight: //turn right
elisham11 0:2266b80b252e 160 {
elisham11 0:2266b80b252e 161 char recv = Bluetooth.getc();
elisham11 0:2266b80b252e 162 //sonarOn = false;
elisham11 0:2266b80b252e 163 if (recv == '1')
elisham11 0:2266b80b252e 164 {
elisham11 0:2266b80b252e 165 heading = 'r';
elisham11 0:2266b80b252e 166 m_left.speed(0.3);
elisham11 0:2266b80b252e 167 m_right.speed(-0.3);
elisham11 0:2266b80b252e 168 }
elisham11 0:2266b80b252e 169 else if (recv == '0')
elisham11 0:2266b80b252e 170 {
elisham11 0:2266b80b252e 171 heading = '0';
elisham11 0:2266b80b252e 172 m_left.speed(0.0);
elisham11 0:2266b80b252e 173 m_right.speed(0.0);
elisham11 0:2266b80b252e 174 }
elisham11 0:2266b80b252e 175 else state = start;
elisham11 0:2266b80b252e 176 }
elisham11 0:2266b80b252e 177 break;
elisham11 0:2266b80b252e 178
elisham11 0:2266b80b252e 179 default:
elisham11 0:2266b80b252e 180 Bluetooth.getc();
elisham11 0:2266b80b252e 181 state = start;
elisham11 0:2266b80b252e 182 }
elisham11 0:2266b80b252e 183 }
elisham11 0:2266b80b252e 184
elisham11 0:2266b80b252e 185 //To see serial output on Mac, run ls /dev/tty.usbmodem* once the mbed is connected
elisham11 0:2266b80b252e 186 //to find its location. Then run screen <mbed location> to see serial output.
elisham11 0:2266b80b252e 187 int Left[1000];
elisham11 0:2266b80b252e 188 float Time[1000];
elisham11 0:2266b80b252e 189 char Head[1000];
elisham11 0:2266b80b252e 190 float Light[1000];
elisham11 0:2266b80b252e 191 float Mag[1000];
elisham11 0:2266b80b252e 192 int j = 0;
elisham11 0:2266b80b252e 193 int k = 0;
elisham11 0:2266b80b252e 194 volatile bool startTime = false;
elisham11 0:2266b80b252e 195
elisham11 0:2266b80b252e 196 void distL(int distance)
elisham11 0:2266b80b252e 197 {
elisham11 0:2266b80b252e 198 if (sonarOn && startTime)
elisham11 0:2266b80b252e 199 {
elisham11 0:2266b80b252e 200 Left[j] = distance;
elisham11 0:2266b80b252e 201 Time[j] = t.read();
elisham11 0:2266b80b252e 202 Head[j] = heading;
elisham11 0:2266b80b252e 203 Light[j] = (float)photocell;
elisham11 0:2266b80b252e 204 j++;
elisham11 0:2266b80b252e 205 printf("Distance %i mm, Time %f s, Light %0.3f ,Heading %c \r\n", distance, t.read(), (float)photocell, heading);//code here to execute when distance changes
elisham11 0:2266b80b252e 206 }
elisham11 0:2266b80b252e 207 }
elisham11 0:2266b80b252e 208 //IMU
elisham11 0:2266b80b252e 209 void printAttitude(float ax, float ay, float az, float mx, float my, float mz)
elisham11 0:2266b80b252e 210 {
elisham11 0:2266b80b252e 211 if(sonarOn && startTime)
elisham11 0:2266b80b252e 212 {
elisham11 0:2266b80b252e 213 float roll = atan2(ay, az);
elisham11 0:2266b80b252e 214 float pitch = atan2(-ax, sqrt(ay * ay + az * az));
elisham11 0:2266b80b252e 215 // touchy trig stuff to use arctan to get compass heading (scale is 0..360)
elisham11 0:2266b80b252e 216 mx = -mx;
elisham11 0:2266b80b252e 217 float heading;
elisham11 0:2266b80b252e 218 if (my == 0.0)
elisham11 0:2266b80b252e 219 heading = (mx < 0.0) ? 180.0 : 0.0;
elisham11 0:2266b80b252e 220 else
elisham11 0:2266b80b252e 221 heading = atan2(mx, my)*360.0/(2.0*PI);
elisham11 0:2266b80b252e 222 //pc.printf("heading atan=%f \n\r",heading);
elisham11 0:2266b80b252e 223 heading -= DECLINATION; //correct for geo location
elisham11 0:2266b80b252e 224 if(heading>180.0) heading = heading - 360.0;
elisham11 0:2266b80b252e 225 else if(heading<-180.0) heading = 360.0 + heading;
elisham11 0:2266b80b252e 226 else if(heading<0.0) heading = 360.0 + heading;
elisham11 0:2266b80b252e 227
elisham11 0:2266b80b252e 228 // Convert everything from radians to degrees:
elisham11 0:2266b80b252e 229 //heading *= 180.0 / PI;
elisham11 0:2266b80b252e 230 pitch *= 180.0 / PI;
elisham11 0:2266b80b252e 231 roll *= 180.0 / PI;
elisham11 0:2266b80b252e 232
elisham11 0:2266b80b252e 233 //pc.printf("Pitch: %f, Roll: %f degress\n\r",pitch,roll);
elisham11 0:2266b80b252e 234 pc.printf("Magnetic Heading: %f degress\n\r",heading);
elisham11 0:2266b80b252e 235 wait(0.3);
elisham11 0:2266b80b252e 236 Mag[k] = heading;
elisham11 0:2266b80b252e 237 k++;
elisham11 0:2266b80b252e 238 }
elisham11 0:2266b80b252e 239 }
elisham11 0:2266b80b252e 240 //sample 10 times every 3 seconds
elisham11 0:2266b80b252e 241 ultrasonic mu_L(p11, p12, .3, 1, &distL); //Set the trigger pin to D8 and the echo pin to D9
elisham11 0:2266b80b252e 242 //have updates every .1 seconds and a timeout after 1
elisham11 0:2266b80b252e 243 //second, and call dist when the distance changes
elisham11 0:2266b80b252e 244
elisham11 0:2266b80b252e 245
elisham11 0:2266b80b252e 246
elisham11 0:2266b80b252e 247 int main()
elisham11 0:2266b80b252e 248 {
elisham11 0:2266b80b252e 249 LSM9DS1 IMU(p28, p27, 0xD6, 0x3C);
elisham11 0:2266b80b252e 250 led = 0.0
elisham11 0:2266b80b252e 251 IMU.begin();
elisham11 0:2266b80b252e 252 if (!IMU.begin()) {
elisham11 0:2266b80b252e 253 pc.printf("Failed to communicate with LSM9DS1.\n");
elisham11 0:2266b80b252e 254 }
elisham11 0:2266b80b252e 255 IMU.calibrate(1);
elisham11 0:2266b80b252e 256 IMU.calibrateMag(0);
elisham11 0:2266b80b252e 257 led1 = 1.0;
elisham11 0:2266b80b252e 258 wait(0.5);
elisham11 0:2266b80b252e 259 led1 = 0.0;
elisham11 0:2266b80b252e 260 //t.start(); //start timer
elisham11 0:2266b80b252e 261 mu_L.startUpdates(); //start measuring the distance with sonar sensor
elisham11 0:2266b80b252e 262 Bluetooth.attach(&parse_message,Serial::RxIrq); //attach bluetooth interrupt
elisham11 0:2266b80b252e 263
elisham11 0:2266b80b252e 264 while(running)
elisham11 0:2266b80b252e 265 {
elisham11 0:2266b80b252e 266 mu_L.checkDistance(); //call checkDistance() as much as possible, as this is where
elisham11 0:2266b80b252e 267 //the class checks if dist needs to be called.
elisham11 0:2266b80b252e 268 //Start the timer only once sonar has turned on and the timer has not been previously started
elisham11 0:2266b80b252e 269 while(!IMU.tempAvailable());
elisham11 0:2266b80b252e 270 IMU.readTemp();
elisham11 0:2266b80b252e 271 while(!IMU.magAvailable(X_AXIS));
elisham11 0:2266b80b252e 272 IMU.readMag();
elisham11 0:2266b80b252e 273 while(!IMU.accelAvailable());
elisham11 0:2266b80b252e 274 IMU.readAccel();
elisham11 0:2266b80b252e 275 while(!IMU.gyroAvailable());
elisham11 0:2266b80b252e 276 IMU.readGyro();
elisham11 0:2266b80b252e 277 printAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx),
elisham11 0:2266b80b252e 278 IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
elisham11 0:2266b80b252e 279 if (sonarOn && !startTime)
elisham11 0:2266b80b252e 280 {
elisham11 0:2266b80b252e 281 startTime = true;
elisham11 0:2266b80b252e 282 t.start();
elisham11 0:2266b80b252e 283
elisham11 0:2266b80b252e 284 }
elisham11 0:2266b80b252e 285 }
elisham11 0:2266b80b252e 286
elisham11 0:2266b80b252e 287 //Store values into SD card
elisham11 0:2266b80b252e 288 //Open a new file to store the data
elisham11 0:2266b80b252e 289 mkdir("/sd/mydir", 0777);
elisham11 0:2266b80b252e 290 FILE *left = fopen("/sd/mydir/left.txt", "w");
elisham11 0:2266b80b252e 291 if(left == NULL) //If there is an error in opening the file, throw error
elisham11 0:2266b80b252e 292 {
elisham11 0:2266b80b252e 293 error("Could not open file left for write\n");
elisham11 0:2266b80b252e 294 }
elisham11 0:2266b80b252e 295 fprintf(left, "sonar, \t time, \t light, \t heading \n");
elisham11 0:2266b80b252e 296 //Loop through array values and store into each line of text
elisham11 0:2266b80b252e 297 for (int i=0; i<1000; i++)
elisham11 0:2266b80b252e 298 {
elisham11 0:2266b80b252e 299
elisham11 0:2266b80b252e 300 fprintf(left, "%i, %0.4f, %0.4f, %0.4f, %c \n", Left[i], Time[i],Light[i] , Mag[i], Head[i]);
elisham11 0:2266b80b252e 301 }
elisham11 0:2266b80b252e 302 //Close file
elisham11 0:2266b80b252e 303 fclose(left);
elisham11 0:2266b80b252e 304 }