Diff: main.cpp

Revision:

19:9ea181b003af

Parent:

18:11883c581785

Child:

20:6b5f83412413

--- a/main.cpp	Thu Apr 23 15:46:11 2020 +0000
+++ b/main.cpp	Thu Apr 23 15:56:22 2020 +0000
@@ -36,6 +36,7 @@
 void get_current_angle();
 
 float angleBiasGyro = 0;
+float angleBiasAcc = 0;
 
 
 ///////////////////////////////////////////////////////////////////////////////
@@ -140,7 +141,7 @@
 void calibrate_imu() {
     for(int i = 0; i < 500; i++){
         imu.readGyro();
-        angleBiasGyro += imu.gy;
+        angleBiasGyro += imu.calcGyro(imu.gy);
         angleBiasAcc += (-1)*atan2(imu.ax,imu.az)*180/3.142-90
     }
     angleBiasGyro /= 500;
@@ -150,11 +151,11 @@
 void get_current_angle() {
 //    return;
     imu.readGyro();
-    int gyro = -(imu.gy-angleBiasGyro) * .01;
+    int gyro = -(imu.calcGyro(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 += .98*gyro + 0.02*acc;
+    pAngle += 1*gyro //+ 0.02*acc;
     angleMutex.unlock();
 }