diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIO.java b/src/main/java/frc/robot/subsystems/drive/GyroIO.java index d140ebec..3d9a2829 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIO.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIO.java @@ -4,7 +4,7 @@ public interface GyroIO { @AutoLog - public static class GyroIOInputs { + class GyroIOInputs { public boolean connected = false; public double yawPositionRad = 0; public double pitchPositionRad = 0; @@ -14,9 +14,11 @@ public static class GyroIOInputs { public double rollVelocityRadPerSec = 0.0; } - public default void updateInputs(GyroIOInputs inputs) {} + default void updateInputs(GyroIOInputs inputs) {} - public default double getYawDegrees() { - return 0; + default void setGyro(GyroIOInputs inputs, double degrees) { + inputs.yawPositionRad = Math.toRadians(degrees); } + + default void resetGyro() {} } diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOADIS16470.java b/src/main/java/frc/robot/subsystems/drive/GyroIOADIS16470.java index 47b61ee5..16e8e93f 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOADIS16470.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOADIS16470.java @@ -37,4 +37,13 @@ public void updateInputs(GyroIOInputs gyroInputs) { gyroInputs.rollVelocityRadPerSec = Units.degreesToRadians(gyro.getRate(ADIS16470_IMU.IMUAxis.kZ)); } + + public void setGyro(GyroIOInputs inputs, double degrees) { + gyro.setGyroAngle(ADIS16470_IMU.IMUAxis.kZ, Math.toRadians(degrees)); + inputs.yawPositionRad = Math.toRadians(degrees); + } + + public void resetGyro() { + gyro.reset(); + } }