Reset and Set Gyro methods

Mechanical-Advantage · Feb 11, 2024 · c592a76 · c592a76
1 parent 0434ce2
commit c592a76
Show file tree

Hide file tree

Showing 2 changed files with 15 additions and 4 deletions.
diff --git 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
@@ -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();
+  }
 }