Diff: IMU.cpp

Revision:

2:ae1bfb236387

Parent:

1:419c1baddb68

Child:

4:ab08a70e0d7d

diff -r 419c1baddb68 -r ae1bfb236387 IMU.cpp
--- a/IMU.cpp	Thu Feb 09 20:14:25 2017 +0000
+++ b/IMU.cpp	Thu Feb 16 18:25:00 2017 +0000
@@ -1,1 +1,66 @@
-//edit
\ No newline at end of file
+#include "mbed.h"
+#include "LSM9DS1.h"
+#include "IMU.h"
+
+#define PI 3.1514
+
+IMU::IMU() {
+
+    LSM9DS1 I(p9, p10, 0xD6, 0x3C);
+    I.begin();
+    if (!I.begin()) {
+        printf("Failed to communicate with LSM9DS1.\n");
+    }
+    I.calibrate(1);
+    bool state;
+    float ax01 = I.calcAccel(I.ax);
+    float ay01 = I.calcAccel(I.ay);
+    float az01 = I.calcAccel(I.az);
+    float ax0 = ax01;
+    float ay0 = ay01;
+    float az0 = az01; 
+    float ax;
+    float ay;
+    float az;
+    float theta;
+}
+    
+void IMU::trackBed()
+{
+    if (state != 0) {
+        return;
+    }
+    else {
+        while(state == 0) {
+        
+            while(!I.accelAvailable());
+            I.readAccel();
+        
+             // current a = <ax,ay,az> 
+            ax = I.calcAccel(I.ax);
+            ay = I.calcAccel(I.ay);
+            az = I.calcAccel(I.az);
+            // theta in degrees between a0 and a
+            theta = acos((ax0*ax + ay0*ay + az0*az)/(sqrt(ax0*ax0+ay0*ay0+az0*az0)*sqrt(ax*ax+ay*ay+az*az)))*180/PI;
+        }
+    }
+}
+        
+void IMU::trackHeading()
+{
+    if (state != 1) {
+        return;
+    }
+    else {
+        while (state == 1) {
+            
+            while(!I.accelAvailable());
+            
+            I.readAccel();
+            ax0 = I.calcAccel(I.ax);
+            ay0 = I.calcAccel(I.ay);
+            az0 = I.calcAccel(I.az);
+        }
+    }
+}
+            
\ No newline at end of file