Diff: main.cpp

Revision:

3:828e82089564

Parent:

2:c041e434eab6

Child:

4:3040d0f9e8c6

diff -r c041e434eab6 -r 828e82089564 main.cpp
--- a/main.cpp	Thu Mar 31 20:47:04 2016 +0000
+++ b/main.cpp	Thu Mar 31 22:05:57 2016 +0000
@@ -3,6 +3,7 @@
 #include "Adafruit_9DOF.h"
 #include "Serial_base.h"
 #include "controller.h"
+#include "sensor.h"
 
 DigitalOut myled(LED1);
 Adafruit_9DOF dof = Adafruit_9DOF();
@@ -12,59 +13,21 @@
 
 Serial pc(USBTX, USBRX);
 
-/* Offsets of gyro at rest */
-float x_offset = 0, y_offset = 0, z_offset = 0;
-
-void initSensors()
-{
-    s_com->println(("\r\nInitializing sensors!"));
-    if (!accel.begin()) {
-        /* There was a problem detecting the LSM303 ... check your connections */
-        s_com->println(("Ooops, no LSM303 accel detected ... Check your wiring!"));
-        while(1);
-    }
-    if (!mag.begin()) {
-        /* There was a problem detecting the LSM303 ... check your connections */
-        s_com->println("Ooops, no LSM303 mag detected ... Check your wiring!");
-        while(1);
-    }
-    if (!gyro.begin(GYRO_RANGE_2000DPS)) {
-        /* There was a problem detecting the L3GD20 ... check your connections */
-        s_com->println("Ooops, no L3GD20 gyro detected ... Check your wiring or I2C ADDR!");
-        while(1);
-    }
-    /* Calculate initial offsets and noise level of gyro */
-    float sampleNum = 500;
-    sensors_event_t gyro_event;
-    for (int n = 0; n < sampleNum; n++) {
-        gyro.getEvent(&gyro_event);
-        x_offset += gyro_event.gyro.x;
-        y_offset += gyro_event.gyro.y;
-        z_offset += gyro_event.gyro.z;
-    }
-    x_offset = x_offset / sampleNum;
-    y_offset = y_offset / sampleNum;
-    z_offset = z_offset / sampleNum;
-    s_com->print("Offsets... X: ");
-    s_com->print(x_offset);
-    s_com->print("\tY: ");
-    s_com->print(y_offset);
-    s_com->print("\tZ: ");
-    s_com->print(z_offset);
-}
-
 
 
 int main()
 {
-    initSensors();
+    control result;
+    offset offset_gyro; 
+
+    initSensors(accel, mag, gyro,offset_gyro);
 
     sensors_event_t accel_event;
     sensors_event_t mag_event;
     sensors_event_t gyro_event;
     sensors_vec_t   orientation;
     
-    control result;
+
 
     while (1) {
 
@@ -89,9 +52,9 @@
         }
         /* Get angular rate data from gyroscope */
         gyro.getEvent(&gyro_event);
-        gyro_event.gyro.x -= x_offset;
-        gyro_event.gyro.y -= y_offset;
-        gyro_event.gyro.z -= z_offset;
+        gyro_event.gyro.x -= offset_gyro.x_offset;
+        gyro_event.gyro.y -= offset_gyro.y_offset;
+        gyro_event.gyro.z -= offset_gyro.z_offset;
 
         wait(0.1);