diff options
Diffstat (limited to 'src/main/java/frc/robot/util')
| -rw-r--r-- | src/main/java/frc/robot/util/IMUGyro.java | 34 |
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() |