Elisha Mang
ECE 4180 Final Project - Georgia Institute of Technology by LZ and EM
Dependencies: mbed Motor BNO055 SDFileSystem LSM9DS1_Library_cal HC_SR04_Ultrasonic_Library
Revision 0:2266b80b252e, committed 2019-12-03
- Comitter:
- elisham11
- Date:
- Tue Dec 03 18:57:39 2019 +0000
- Commit message:
- 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.
Changed in this revision
diff -r 000000000000 -r 2266b80b252e BNO055.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/BNO055.lib Tue Dec 03 18:57:39 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/vargham/code/BNO055/#f5880506defe
diff -r 000000000000 -r 2266b80b252e HC_SR04_Ultrasonic_Library.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/HC_SR04_Ultrasonic_Library.lib Tue Dec 03 18:57:39 2019 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/ejteb/code/HC_SR04_Ultrasonic_Library/#e0f9c9fb4cf3
diff -r 000000000000 -r 2266b80b252e LSM9DS1_Library_cal.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/LSM9DS1_Library_cal.lib Tue Dec 03 18:57:39 2019 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/users/4180_1/code/LSM9DS1_Library_cal/#36abf8e18ade
diff -r 000000000000 -r 2266b80b252e Motor.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Motor.lib Tue Dec 03 18:57:39 2019 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/simon/code/Motor/#f265e441bcd9
diff -r 000000000000 -r 2266b80b252e SDFileSystem.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/SDFileSystem.lib Tue Dec 03 18:57:39 2019 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/SDFileSystem/#c8f66dc765d4
diff -r 000000000000 -r 2266b80b252e main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Dec 03 18:57:39 2019 +0000
@@ -0,0 +1,304 @@
+#include "mbed.h"
+#include "ultrasonic.h"
+#include "SDFileSystem.h"
+#include <string>
+#include "Motor.h"
+#include "LSM9DS1.h"
+#define PI 3.14159
+#define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA.
+Serial pc(USBTX, USBRX);
+
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+
+//H-bridge
+Motor m_left(p21, p15, p16); // pwm,fwd,rev pwmA
+Motor m_right(p22, p19, p20); // pwm,fwd,rev
+
+//Photo transistor
+AnalogIn photocell(p18);
+
+//Timer
+Timer t; //t.start(), t.stop(), t.read()
+
+//SD Card
+SDFileSystem sd(p5, p6, p7, p8, "sd");
+
+RawSerial Bluetooth(p13,p14); // tx, rx
+
+//Global variables for bluetooth control
+bool sonarOn = false; //Indicates when sonar is taking measurements
+char heading = '0'; //Indicates rough direction of the robot; 'f' = forward, 'l' = left, 'r' = right, 'b' = backwards, '0' = not moving
+bool running = true; //Keeps main while loop going
+
+
+//Code for interrupt routine for Bluetooth input
+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};
+statetype state = start;
+//Interrupt routine to parse message with one new character per serial RX interrupt
+void parse_message()
+{
+ switch (state)
+ {
+ case start:
+ if (Bluetooth.getc()=='!')
+ {
+ //led2 = 1;
+ state = got_exclm;
+ }
+ else state = start;
+ break;
+ case got_exclm:
+
+ if (Bluetooth.getc() == 'B')
+ {
+ state = got_B;
+ }
+ else state = start;
+ break;
+ case got_B:
+ {
+ char recv = Bluetooth.getc();
+ if (recv == '1') state = got_one;
+ else if (recv == '2') state = got_two;
+ else if (recv == '3') state = got_three;
+ else if (recv == '5') state = got_five;
+ else if (recv == '6') state = got_six;
+ else if (recv == '7') state = got_seven;
+ else if (recv == '8') state = got_eight;
+ else state = start;
+ }
+ break;
+ case got_one:
+ if (Bluetooth.getc() == '1')
+ {
+ //Make sure motors are stopped
+ m_left.speed(0.0);
+ m_right.speed(0.0);
+ sonarOn = false;
+ heading = '0';
+ running = false;
+ }
+ else state = start;
+ break;
+ case got_two: //Stop everything
+ if (Bluetooth.getc() == '1')
+ {
+
+ }
+ else state = start;
+ break;
+ case got_three: //currently unimplemented
+ if (Bluetooth.getc() == '1')
+ {
+
+ }
+ else state = start;
+ break;
+ case got_five: //up arrow pressed
+ {
+ char recv = Bluetooth.getc();
+ if (recv == '1') //Button pressed or held
+ {
+ sonarOn = true;
+ heading = 'f';
+ m_left.speed(0.42);
+ m_right.speed(0.4);
+ }
+ else if (recv == '0') //Button released
+ {
+ //sonarOn = false;
+ heading = '0';
+ m_left.speed(0.0);
+ m_right.speed(0.0);
+ }
+ else state = start;
+ }
+ break;
+ case got_six: //down arrow pressed
+ {
+ char recv = Bluetooth.getc();
+ if (recv == '1') //Button pressed or held
+ {
+ sonarOn = true;
+ heading = 'b';
+ m_left.speed(-0.42);
+ m_right.speed(-0.4);
+ }
+ else if (recv == '0')
+ {
+ //sonarOn = false;
+ heading = '0';
+ m_left.speed(0.0);
+ m_right.speed(0.0);
+ }
+ else state = start;
+ }
+ break;
+ case got_seven: //turn left
+ {
+ char recv = Bluetooth.getc();
+ //sonarOn = false;
+ if (recv == '1')
+ {
+ heading = 'l';
+ m_left.speed(-0.3);
+ m_right.speed(0.3);
+ }
+ else if (recv == '0')
+ {
+ heading = '0';
+ m_left.speed(0.0);
+ m_right.speed(0.0);
+ }
+ else state = start;
+ }
+ break;
+ case got_eight: //turn right
+ {
+ char recv = Bluetooth.getc();
+ //sonarOn = false;
+ if (recv == '1')
+ {
+ heading = 'r';
+ m_left.speed(0.3);
+ m_right.speed(-0.3);
+ }
+ else if (recv == '0')
+ {
+ heading = '0';
+ m_left.speed(0.0);
+ m_right.speed(0.0);
+ }
+ else state = start;
+ }
+ break;
+
+ default:
+ Bluetooth.getc();
+ state = start;
+ }
+}
+
+//To see serial output on Mac, run ls /dev/tty.usbmodem* once the mbed is connected
+//to find its location. Then run screen <mbed location> to see serial output.
+int Left[1000];
+float Time[1000];
+char Head[1000];
+float Light[1000];
+float Mag[1000];
+int j = 0;
+int k = 0;
+volatile bool startTime = false;
+
+void distL(int distance)
+{
+ if (sonarOn && startTime)
+ {
+ Left[j] = distance;
+ Time[j] = t.read();
+ Head[j] = heading;
+ Light[j] = (float)photocell;
+ j++;
+ 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
+ }
+}
+//IMU
+void printAttitude(float ax, float ay, float az, float mx, float my, float mz)
+{
+ if(sonarOn && startTime)
+ {
+ float roll = atan2(ay, az);
+ float pitch = atan2(-ax, sqrt(ay * ay + az * az));
+// touchy trig stuff to use arctan to get compass heading (scale is 0..360)
+ mx = -mx;
+ float heading;
+ if (my == 0.0)
+ heading = (mx < 0.0) ? 180.0 : 0.0;
+ else
+ heading = atan2(mx, my)*360.0/(2.0*PI);
+ //pc.printf("heading atan=%f \n\r",heading);
+ heading -= DECLINATION; //correct for geo location
+ if(heading>180.0) heading = heading - 360.0;
+ else if(heading<-180.0) heading = 360.0 + heading;
+ else if(heading<0.0) heading = 360.0 + heading;
+
+ // Convert everything from radians to degrees:
+ //heading *= 180.0 / PI;
+ pitch *= 180.0 / PI;
+ roll *= 180.0 / PI;
+
+ //pc.printf("Pitch: %f, Roll: %f degress\n\r",pitch,roll);
+ pc.printf("Magnetic Heading: %f degress\n\r",heading);
+ wait(0.3);
+ Mag[k] = heading;
+ k++;
+ }
+}
+//sample 10 times every 3 seconds
+ultrasonic mu_L(p11, p12, .3, 1, &distL); //Set the trigger pin to D8 and the echo pin to D9
+ //have updates every .1 seconds and a timeout after 1
+ //second, and call dist when the distance changes
+
+
+
+int main()
+{
+ LSM9DS1 IMU(p28, p27, 0xD6, 0x3C);
+ led = 0.0
+ IMU.begin();
+ if (!IMU.begin()) {
+ pc.printf("Failed to communicate with LSM9DS1.\n");
+ }
+ IMU.calibrate(1);
+ IMU.calibrateMag(0);
+ led1 = 1.0;
+ wait(0.5);
+ led1 = 0.0;
+ //t.start(); //start timer
+ mu_L.startUpdates(); //start measuring the distance with sonar sensor
+ Bluetooth.attach(&parse_message,Serial::RxIrq); //attach bluetooth interrupt
+
+ while(running)
+ {
+ mu_L.checkDistance(); //call checkDistance() as much as possible, as this is where
+ //the class checks if dist needs to be called.
+ //Start the timer only once sonar has turned on and the timer has not been previously started
+ while(!IMU.tempAvailable());
+ IMU.readTemp();
+ while(!IMU.magAvailable(X_AXIS));
+ IMU.readMag();
+ while(!IMU.accelAvailable());
+ IMU.readAccel();
+ while(!IMU.gyroAvailable());
+ IMU.readGyro();
+ printAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx),
+ IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
+ if (sonarOn && !startTime)
+ {
+ startTime = true;
+ t.start();
+
+ }
+ }
+
+ //Store values into SD card
+ //Open a new file to store the data
+ mkdir("/sd/mydir", 0777);
+ FILE *left = fopen("/sd/mydir/left.txt", "w");
+ if(left == NULL) //If there is an error in opening the file, throw error
+ {
+ error("Could not open file left for write\n");
+ }
+ fprintf(left, "sonar, \t time, \t light, \t heading \n");
+ //Loop through array values and store into each line of text
+ for (int i=0; i<1000; i++)
+ {
+
+ fprintf(left, "%i, %0.4f, %0.4f, %0.4f, %c \n", Left[i], Time[i],Light[i] , Mag[i], Head[i]);
+ }
+ //Close file
+ fclose(left);
+}
diff -r 000000000000 -r 2266b80b252e mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Tue Dec 03 18:57:39 2019 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/824293ae5e43 \ No newline at end of file