Better use of gyro in re-zeroing process

src/main/java/frc/robot/RobotContainer.java

@@ -16,7 +16,6 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
-import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.constants.GeneralConstants;
@@ -264,15 +263,17 @@ private void configureBindings() {
 
     controller
         .start()
-        .whileTrue(
-            new RunCommand(
+        .onTrue(
+            runOnce(
                 () ->
                     drive.setYaw(
                         switch (DriverStation.getAlliance().get()) {
                           case Blue -> Rotation2d.fromDegrees(0);
                           case Red -> Rotation2d.fromDegrees(180);
                           default -> Rotation2d.fromDegrees(0);
                         })));
+
+    controller.back().onTrue(runOnce(drive::zeroToGyro));
   }
 
   private boolean IsEndGame() {

src/main/java/frc/robot/subsystems/drive/Drive.java

@@ -367,15 +367,21 @@ public double getRawGyroYaw() {
 
   /** Resets the current odometry pose. */
   public void setPose(Pose2d pose) {
-    poseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose);
+    gyroIO.setYaw(pose.getRotation());
+    // We just pass the desired pose for gyro yaw, since we've just re-zeroed it.
+    poseEstimator.resetPosition(pose.getRotation(), getModulePositions(), pose);
   }
 
   /** Sets the current odometry yaw. */
   public void setYaw(Rotation2d yaw) {
-    gyroIO.setYaw(yaw);
     setPose(new Pose2d(getPose().getTranslation(), yaw));
   }
 
+  /** Quick and dirty method of "re-zeroing". Sets odometry rotation to gyro reported rotation. */
+  public void zeroToGyro() {
+    setPose(new Pose2d(getPose().getTranslation(), gyroInputs.yawPosition));
+  }
+
   /** Returns an array of module translations. */
   public static Translation2d[] getModuleTranslations() {
     return new Translation2d[] {

src/main/java/frc/robot/subsystems/drive/GyroIONavX.java

@@ -16,20 +16,29 @@
 import static frc.robot.constants.DriveConstants.Module.ODOMETRY_FREQUENCY;
 import static frc.robot.constants.GeneralConstants.currentRobot;
 
-import com.kauailabs.navx.frc.AHRS;
+import com.studica.frc.AHRS;
+import com.studica.frc.AHRS.NavXComType;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.SPI;
 import java.util.Queue;
 import org.littletonrobotics.junction.Logger;
 
 /** IO implementation for NavX */
 public class GyroIONavX implements GyroIO {
-  private final AHRS navX = new AHRS(SPI.Port.kMXP, (byte) ODOMETRY_FREQUENCY); // 200Hz update rate
+  // TODO NavX documentation is not good and running into a lot of issues. Please get Pidgeon 2 next
+  // year
+  private final AHRS navX =
+      new AHRS(NavXComType.kMXP_SPI, (int) ODOMETRY_FREQUENCY); // 200Hz update rate
   private final Queue<Double> yawPositionQueue;
   private final Queue<Double> yawTimestampQueue;
 
   public GyroIONavX() {
+    while (navX.isCalibrating()) {
+      System.out.println("Waiting for NavX calibration");
+      Logger.recordOutput("Gyro/NavXCalibrating", true);
+      Thread.yield();
+    }
+    Logger.recordOutput("Gyro/NavXCalibrating", false);
     navX.reset();
     yawPositionQueue = SparkMaxOdometryThread.getInstance().registerSignal(() -> navX.getAngle());
     yawTimestampQueue =
@@ -54,9 +63,8 @@ public void updateInputs(GyroIOInputs inputs) {
                 (Double value) ->
                     Rotation2d.fromDegrees(
                         switch (currentRobot) {
-                          case DORY ->
-                              -value; // According to last years code, clef NavX is inverted
-                          case NAUTILUS -> -value;
+                          case DORY -> value;
+                          case NAUTILUS -> value;
                         }))
             .toArray(Rotation2d[]::new);
     yawTimestampQueue.clear();
@@ -65,4 +73,11 @@ public void updateInputs(GyroIOInputs inputs) {
     Logger.recordOutput("NavX Rotation", navX.getRotation2d());
     Logger.recordOutput("NavX Last Sensor Timestamp", navX.getLastSensorTimestamp());
   }
+
+  @Override
+  public void setYaw(Rotation2d yaw) {
+    navX.setAngleAdjustment(0.0); // Reset angle adjustment so we can rezero
+
+    navX.setAngleAdjustment(yaw.getDegrees() - navX.getAngle());
+  }
 }

vendordeps/Studica-2025.0.1.json

@@ -0,0 +1,71 @@
+{
+  "fileName": "Studica-2025.0.1.json",
+  "name": "Studica",
+  "version": "2025.0.1",
+  "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b",
+  "frcYear": "2025",
+  "mavenUrls": [
+    "https://dev.studica.com/maven/release/2025/"
+  ],
+  "jsonUrl": "https://dev.studica.com/releases/2025/Studica-2025.0.1.json",
+  "cppDependencies": [
+    {
+      "artifactId": "Studica-cpp",
+      "binaryPlatforms": [
+        "linuxathena",
+        "linuxarm32",
+        "linuxarm64",
+        "linuxx86-64",
+        "osxuniversal",
+        "windowsx86-64"
+      ],
+      "groupId": "com.studica.frc",
+      "headerClassifier": "headers",
+      "libName": "Studica",
+      "sharedLibrary": false,
+      "skipInvalidPlatforms": true,
+      "version": "2025.0.1"
+    },
+    {
+      "artifactId": "Studica-driver",
+      "binaryPlatforms": [
+        "linuxathena",
+        "linuxarm32",
+        "linuxarm64",
+        "linuxx86-64",
+        "osxuniversal",
+        "windowsx86-64"
+      ],
+      "groupId": "com.studica.frc",
+      "headerClassifier": "headers",
+      "libName": "StudicaDriver",
+      "sharedLibrary": false,
+      "skipInvalidPlatforms": true,
+      "version": "2025.0.1"
+    }
+  ],
+  "javaDependencies": [
+    {
+      "artifactId": "Studica-java",
+      "groupId": "com.studica.frc",
+      "version": "2025.0.1"
+    }
+  ],
+  "jniDependencies": [
+    {
+      "artifactId": "Studica-driver",
+      "groupId": "com.studica.frc",
+      "isJar": false,
+      "skipInvalidPlatforms": true,
+      "validPlatforms": [
+        "linuxathena",
+        "linuxarm32",
+        "linuxarm64",
+        "linuxx86-64",
+        "osxuniversal",
+        "windowsx86-64"
+      ],
+      "version": "2025.0.1"
+    }
+  ]
+}