Diff: IMU.cpp

Revision:

4:ab08a70e0d7d

Parent:

2:ae1bfb236387

--- a/IMU.cpp	Thu Feb 16 18:28:00 2017 +0000
+++ b/IMU.cpp	Thu Feb 16 20:31:35 2017 +0000
@@ -1,30 +1,24 @@
-#include "mbed.h"
-#include "LSM9DS1.h"
 #include "IMU.h"
 
-#define PI 3.1514
+#define PI 3.14159
 
 IMU::IMU() {
-
-    LSM9DS1 I(p9, p10, 0xD6, 0x3C);
-    I.begin();
-    if (!I.begin()) {
+    I = new LSM9DS1(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;
+    I->calibrate(1);
+    //bool state;
+    //Do we want to give state a starting value?
+    ax01 = I->calcAccel(I->ax);
+    ay01 = I->calcAccel(I->ay);
+    az01 = I->calcAccel(I->az);
+    ax0 = ax01;
+    ay0 = ay01;
+    az0 = az01;
 }
-    
+
 void IMU::trackBed()
 {
     if (state != 0) {
@@ -32,20 +26,19 @@
     }
     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);
+            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) {
@@ -53,14 +46,13 @@
     }
     else {
         while (state == 1) {
-            
-            while(!I.accelAvailable());
-            
-            I.readAccel();
-            ax0 = I.calcAccel(I.ax);
-            ay0 = I.calcAccel(I.ay);
-            az0 = I.calcAccel(I.az);
+
+            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