tested

1 files changed, 14 insertions, 20 deletions

diff --git a/src/main/java/frc/robot/util/IMUGyro.java b/src/main/java/frc/robot/util/IMUGyro.java
index 1ea1983..3860b85 100644
--- a/src/main/java/frc/robot/util/IMUGyro.java
+++ b/src/main/java/frc/robot/util/IMUGyro.java
@@ -12,7 +12,11 @@ public class IMUGyro
     public double roll;
     public double yaw;
  
-    private double gyroOffsets[] = new double[3];
+    private double gyroOffsets[] = {
+        0.312449302886265,
+        0.04883417185589417,
+        -0.12945104079301352
+    };
     
     // for deltatime
     private double prev_time = Timer.getFPGATimestamp();
@@ -27,7 +31,7 @@ public class IMUGyro
     private final int ACCL_ADDR = 0x3B;
     private final int ACCL_CONFIG = 0x1C;
  
-    private final double SCALE_GYRO = (Math.PI/(180*16.384));
+    private final double SCALE_GYRO = (Math.PI/(180*16.384) * 2 /*??*/);
     private final double SCALE_ACCL = (1/16.384);
  
     public IMUGyro(I2C gyro) 
@@ -35,15 +39,15 @@ public class IMUGyro
         mpu = gyro;
         reset();
 
-        mpu.write(CONFIG_ADDR, 0b00000110);
-        mpu.write(SMPLRT_DIV, 0b00010011);
-        mpu.write(GYRO_CONFIG, 0b00001000);
-        mpu.write(ACCL_CONFIG, 0b00000000);
+        // mpu.write(CONFIG_ADDR, 0b000000);
+        mpu.write(SMPLRT_DIV, 0b00000010);
+        // mpu.write(GYRO_CONFIG, 0b00011000);
+        // mpu.write(ACCL_CONFIG, 0b00000000);
     }
  
     public void reset()
     {
-        mpu.write(PWR_MGMT_1, 0b10001000);
+        mpu.write(PWR_MGMT_1, 0b000001000);
 
         pitch = 0;
         roll = 0;
@@ -56,21 +60,11 @@ public class IMUGyro
         double dt = cur_time - prev_time;
         prev_time = cur_time;
 
-        // double[] gRates = gyroRates();
-
-        // roll = gRates[0] * dt;
-        // pitch = gRates[1] * dt;
-        // yaw = gRates[2] * dt;
-
         double[] gRates = gyroRates();
-        double[] aRates = acclRates();
-
-        mahony(30, 0, gRates, aRates, dt);
-        double[] angles = fromQuaternion(q);
 
-        roll = angles[0]; 
-        pitch = angles[1]; 
-        yaw = angles[2];
+        roll += gRates[0] * dt;
+        pitch += gRates[1] * dt;
+        yaw += gRates[2] * dt;
     }
  
     public Rotation2d toRotation2d()