Diff: main.cpp

Revision:

18:11883c581785

Parent:

17:afde478daa01

Child:

19:9ea181b003af

--- a/main.cpp	Thu Apr 23 15:32:27 2020 +0000
+++ b/main.cpp	Thu Apr 23 15:46:11 2020 +0000
@@ -35,7 +35,7 @@
 
 void get_current_angle();
 
-float angleBias = 0;
+float angleBiasGyro = 0;
 
 
 ///////////////////////////////////////////////////////////////////////////////
@@ -140,18 +140,21 @@
 void calibrate_imu() {
     for(int i = 0; i < 500; i++){
         imu.readGyro();
-        angleBias += imu.gy;
+        angleBiasGyro += imu.gy;
+        angleBiasAcc += (-1)*atan2(imu.ax,imu.az)*180/3.142-90
     }
-    angleBias /= 500;
+    angleBiasGyro /= 500;
+    angleBiasAcc /= 500;
 }
 
 void get_current_angle() {
 //    return;
     imu.readGyro();
-    int gyro = -(imu.gy-angleBias) * .01;
+    int gyro = -(imu.gy-angleBiasGyro) * .01;
+    int acc = (-1)*atan2(imu.ax,imu.az)*180/3.142-90-angleBiasAcc;
     //pc.printf("gyro:%f",gyro);
     angleMutex.lock();
-    pAngle += gyro;
+    pAngle += .98*gyro + 0.02*acc;
     angleMutex.unlock();
 }