Diff: main.cpp

Revision:

12:0fb3a03aeecb

Parent:

11:371026db1dcd

Child:

14:988b8d294259

--- a/main.cpp	Sat Jan 28 05:10:07 2017 +0000
+++ b/main.cpp	Mon Feb 06 20:34:55 2017 +0000
@@ -170,6 +170,22 @@
     //Get IMU instance and configure it
     BMI160_I2C imu(i2cBus, BMI160_I2C::I2C_ADRS_SDO_LO);
     
+    //Power up sensors in normal mode
+    if(imu.setSensorPowerMode(BMI160::GYRO, BMI160::NORMAL) != BMI160::RTN_NO_ERROR)
+    {
+        printf("Failed to set gyroscope power mode\n");
+        failures++;
+    }
+    
+    wait(0.1);
+    
+    if(imu.setSensorPowerMode(BMI160::ACC, BMI160::NORMAL) != BMI160::RTN_NO_ERROR)
+    {
+        printf("Failed to set accelerometer power mode\n");
+        failures++;
+    }
+    wait(0.1);
+    
     BMI160::AccConfig accConfig;
     BMI160::AccConfig accConfigRead;
     accConfig.range = BMI160::SENS_4G;
@@ -246,20 +262,6 @@
         failures++;
     }
     
-    //Power up sensors in normal mode
-    if(imu.setSensorPowerMode(BMI160::GYRO, BMI160::NORMAL) != BMI160::RTN_NO_ERROR)
-    {
-        printf("Failed to set gyroscope power mode\n");
-        failures++;
-    }
-    
-    wait(0.1);
-    
-    if(imu.setSensorPowerMode(BMI160::ACC, BMI160::NORMAL) != BMI160::RTN_NO_ERROR)
-    {
-        printf("Failed to set accelerometer power mode\n");
-        failures++;
-    }
     
     if(failures == 0)
     {
@@ -400,6 +402,7 @@
                     //Error signal
                     currentError = (setPoint - pitch);
                     
+                    //Integral term, dt is included in KI
                     integral = (integral + currentError);
                     
                     //Derivative term, dt is included in KD