Diff: main.cpp

Revision:

17:afde478daa01

Parent:

16:f9e3df933304

Child:

18:11883c581785

Child:

23:d2dcd3f9c309

--- a/main.cpp	Thu Apr 23 15:27:21 2020 +0000
+++ b/main.cpp	Thu Apr 23 15:32:27 2020 +0000
@@ -35,6 +35,8 @@
 
 void get_current_angle();
 
+float angleBias = 0;
+
 
 ///////////////////////////////////////////////////////////////////////////////
 ///////////////////////////// Bluetooth Section ///////////////////////////////
@@ -136,13 +138,17 @@
 ///////////////////////////// IMU STUFF////////////////////////////////////////
 ///////////////////////////////////////////////////////////////////////////////
 void calibrate_imu() {
-    return;
+    for(int i = 0; i < 500; i++){
+        imu.readGyro();
+        angleBias += imu.gy;
+    }
+    angleBias /= 500;
 }
 
 void get_current_angle() {
 //    return;
     imu.readGyro();
-    int gyro = -imu.gy * .01;
+    int gyro = -(imu.gy-angleBias) * .01;
     //pc.printf("gyro:%f",gyro);
     angleMutex.lock();
     pAngle += gyro;
@@ -158,6 +164,7 @@
 
 int main() {
     pc.printf("this is running");
+    calibrate_imu();
     Thread bluetooth;
     Thread system_update;