diff options
Diffstat (limited to 'src/main/java/frc/robot/util')
| -rw-r--r-- | src/main/java/frc/robot/util/IMUGyro.java | 83 |
1 files changed, 3 insertions, 80 deletions
diff --git a/src/main/java/frc/robot/util/IMUGyro.java b/src/main/java/frc/robot/util/IMUGyro.java index 8d41c63..aa4563b 100644 --- a/src/main/java/frc/robot/util/IMUGyro.java +++ b/src/main/java/frc/robot/util/IMUGyro.java @@ -28,21 +28,17 @@ public class IMUGyro private final int GYRO_ADDR = 0x43; private final int GYRO_CONFIG = 0x1B; - private final int ACCL_ADDR = 0x3B; - private final int ACCL_CONFIG = 0x1C; - private final double SCALE_GYRO = (Math.PI/(180*16.384) * 2 /*??*/); - private final double SCALE_ACCL = (1/16.384); public IMUGyro(I2C gyro) { mpu = gyro; reset(); - mpu.write(CONFIG_ADDR, 0b00000100); - mpu.write(SMPLRT_DIV, 0b00010011); + mpu.write(CONFIG_ADDR, 0b00000101); + // mpu.write(SMPLRT_DIV, 0b00010011); mpu.write(GYRO_CONFIG, 0b00011000); - mpu.write(ACCL_CONFIG, 0b00000000); + // mpu.write(ACCL_CONFIG, 0b00000000); } public void reset() @@ -95,11 +91,6 @@ public class IMUGyro return scaledRates(GYRO_ADDR, SCALE_GYRO); } - private double[] acclRates() - { - return scaledRates(ACCL_ADDR, SCALE_ACCL); - } - private double[] scaledRates(int addr, double scale) { byte[] buffer = new byte[6]; @@ -114,73 +105,5 @@ public class IMUGyro { return ((first << 8) | second); } - - // mahony quaternion - private double[] q = {1.0, 0.0, 0.0, 0.0}; - // mahony integral term - private double[] i = new double[3]; - - private double[] fromQuaternion(double[] q) - { - return new double[] { - Math.atan2((q[0] * q[1] + q[2] * q[3]), 0.5 - (q[1] * q[1] + q[2] * q[2])), - Math.asin(2.0 * (q[0] * q[2] - q[1] * q[3])), - -Math.atan2((q[1] * q[2] + q[0] * q[3]), 0.5 - (q[2] * q[2] + q[3] * q[3])) - }; - } - - private void mahony(double Ki, double Kp, double[] gRates, double[] aRates, double dt) - { - double gx = gRates[0], gy = gRates[1], gz = gRates[2]; - double ax = aRates[0], ay = aRates[1], az = aRates[2]; - - double tmp = ax * ax + ay * ay + az * az; - if(tmp > 0.0) - { - tmp = Math.sqrt(tmp); - ax /= tmp; ay /= tmp; az /= tmp; - - // estimated direction of gravity in the body frame - double vx = q[1] * q[3] - q[0] * q[2]; - double vy = q[0] * q[1] + q[2] * q[3]; - double vz = q[0] * q[0] - 0.5f + q[3] * q[3]; - - // error is cross product - double ex = (ay * vz - az * vy); - double ey = (az * vx - ax * vz); - double ez = (ax * vy - ay * vx); - - // integral feedback - if(Ki > 0.0) { - i[0] += Ki * ex * dt; - i[1] += Ki * ey * dt; - i[2] += Ki * ez * dt; - gx += i[0]; - gy += i[1]; - gz += i[2]; - } - - gx += Kp * ex; - gy += Kp * ey; - gz += Kp * ez; - } - - // integrate quaternion - dt /= 2; // ?? - gx *= dt; gy *= dt; gz *= dt; - - double qa = q[0], qb = q[1], qc = q[2], qd = q[3]; - q[0] += (-qb * gx - qc * gy - qd * gz); - q[1] += (qa * gx + qc * gz - qd * gy); - q[2] += (qa * gy - qb * gz + qd * gx); - q[3] += (qa * gz + qb * gy - qc * gx); - - // renormalize - double qnorm_factor = Math.sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); - q[0] /= qnorm_factor; - q[1] /= qnorm_factor; - q[2] /= qnorm_factor; - q[3] /= qnorm_factor; - } } |