Diff: main.cpp

Revision:

1:705baf131710

Parent:

0:e693d5bf0a25

--- a/main.cpp	Wed Feb 03 18:47:07 2016 +0000
+++ b/main.cpp	Fri Apr 14 00:29:47 2017 +0000
@@ -1,5 +1,6 @@
 #include "mbed.h"
 #include "LSM9DS1.h"
+#include "USBJoystick.h"
 #define PI 3.14159
 // Earth's magnetic field varies by location. Add or subtract
 // a declination to get a more accurate heading. Calculate
@@ -9,12 +10,19 @@
 
 DigitalOut myled(LED1);
 Serial pc(USBTX, USBRX);
+DigitalIn pb1(p17);
 // Calculate pitch, roll, and heading.
 // Pitch/roll calculations taken from this app note:
 // http://cache.freescale.com/files/sensors/doc/app_note/AN3461.pdf?fpsp=1
 // Heading calculations taken from this app note:
 // http://www51.honeywell.com/aero/common/documents/myaerospacecatalog-documents/Defense_Brochures-documents/Magnetic__Literature_Application_notes-documents/AN203_Compass_Heading_Using_Magnetometers.pdf
-void printAttitude(float ax, float ay, float az, float mx, float my, float mz)
+float correctHeading(float currHeading, float forward){
+    float newHeading = currHeading - forward;
+    if(newHeading < 0) newHeading = 360 + newHeading;
+    return newHeading;
+}
+
+float printAttitude(float ax, float ay, float az, float mx, float my, float mz)
 {
     float roll = atan2(ay, az);
     float pitch = atan2(-ax, sqrt(ay * ay + az * az));
@@ -37,8 +45,9 @@
     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);
+    //pc.printf("Pitch: %f,    Roll: %f degress\n\r",pitch,roll);
+    //pc.printf("Magnetic Heading: %f degress\n\r",heading);
+    return heading;
 }
 
 
@@ -48,12 +57,21 @@
 {
     //LSM9DS1 lol(p9, p10, 0x6B, 0x1E);
     LSM9DS1 IMU(p28, p27, 0xD6, 0x3C);
+    pb1.mode(PullUp);
     IMU.begin();
+    float forward;
     if (!IMU.begin()) {
         pc.printf("Failed to communicate with LSM9DS1.\n");
     }
     IMU.calibrate(1);
     IMU.calibrateMag(0);
+    pc.printf("Press button to set forward direction");
+    while(pb1 == 1){
+        IMU.readMag();
+        IMU.readAccel();
+        forward = 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));;
+    } 
     while(1) {
         while(!IMU.tempAvailable());
         IMU.readTemp();
@@ -63,17 +81,32 @@
         IMU.readAccel();
         while(!IMU.gyroAvailable());
         IMU.readGyro();
-        pc.printf("\nIMU Temperature = %f C\n\r",25.0 + IMU.temperature/16.0);
+        /*pc.printf("\nIMU Temperature = %f C\n\r",25.0 + IMU.temperature/16.0);
         pc.printf("        X axis    Y axis    Z axis\n\r");
         pc.printf("gyro:  %9f %9f %9f in deg/s\n\r", IMU.calcGyro(IMU.gx), IMU.calcGyro(IMU.gy), IMU.calcGyro(IMU.gz));
         pc.printf("accel: %9f %9f %9f in Gs\n\r", IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az));
         pc.printf("mag:   %9f %9f %9f in gauss\n\r", IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
         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));
-        myled = 1;
-        wait(0.5);
-        myled = 0;
-        wait(0.5);
+                      IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));*/
+      if(abs(IMU.calcGyro(IMU.gy)) > 100){
+           // pc.printf("Forward:%f\n\r",forward);
+            float currHeading = 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));
+           // pc.printf("RawHeading:%f\n\r",currHeading);
+            currHeading = correctHeading(currHeading, forward) + 45;
+          //  pc.printf("CorrectedHeading:%f\n\n\n\r",currHeading);
+            if(currHeading > 360 || currHeading < 90){
+                 pc.printf("Forward\n\r");
+            }
+            else if(currHeading > 90 && currHeading < 180){
+                 pc.printf("Right\n\r");
+            }
+            else if(currHeading > 180 && currHeading < 270){
+                 pc.printf("Backwards\n\r");
+            }
+            else{
+                 pc.printf("Left\n\r");
+            }
+        }
     }
 }