add zero check to modules

Author: mwitcpalek

src/main/java/frc/robot/Robot.java

@@ -57,7 +57,9 @@ public void disabledInit() {
   }
 
   @Override
-  public void disabledPeriodic() {}
+  public void disabledPeriodic() {
+    if (!m_robotContainer.hasSwerveZeroed()) m_robotContainer.zeroSwerve();
+  }
 
   @Override
   public void disabledExit() {}

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

@@ -194,6 +194,14 @@ private void configureTelemetry() {
     telemetryService.start();
   }
 
+  public boolean hasSwerveZeroed() {
+    return driveSubsystem.hasZeroed();
+  }
+
+  public void zeroSwerve() {
+    driveSubsystem.zeroModules();
+  }
+
   private void configureDriverBindings() {
     driveSubsystem.setDefaultCommand(
         new DriveTeleopCommand(

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

@@ -83,6 +83,14 @@ public DriveSubsystem(SwerveIO io) {
     holonomicController.setEnabled(true);
   }
 
+  public boolean hasZeroed() {
+    return inputs.didZero;
+  }
+
+  public void zeroModules() {
+    io.zeroModules();
+  }
+
   // Open-Loop Swerve Movements
   public void drive(double vXmps, double vYmps, double vOmegaRadps) {
     if (!ignoreSticks) {

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

@@ -53,6 +53,7 @@ public class Swerve implements SwerveIO, Checkable {
   private SwerveDriveKinematics kinematics;
   private double fieldY = 0.0;
   private double fieldX = 0.0;
+  private boolean didZero = false;
 
   public Swerve() {
 
@@ -94,6 +95,7 @@ public Swerve() {
               .build();
       swerveModules[i].loadAndSetAzimuthZeroReference();
     }
+    didZero = true;
 
     pigeon = new SF_PIGEON2(DriveConstants.kPigeonCanID, "rio");
     pigeon.applyConfig(DriveConstants.getPigeon2Configuration());
@@ -270,6 +272,13 @@ public void configDriveCurrents(CurrentLimitsConfigs config) {
     }
   }
 
+  @Override
+  public void zeroModules() {
+    for (int i = 0; i < 4; i++) {
+      swerveModules[i].loadAndSetAzimuthZeroReference();
+    }
+  }
+
   @Override
   public void updateInputs(SwerveIOInputs inputs) {
     swerveDrive.updateInputs(); // Call before swerveDrive.periodic()
@@ -299,6 +308,7 @@ public void updateInputs(SwerveIOInputs inputs) {
     inputs.fieldRelSpeed = getFieldRelSpeed(inputs.robotRelSpeed);
     inputs.fieldY = fieldY;
     inputs.fieldX = fieldX;
+    inputs.didZero = didZero;
   }
 
   @Override

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

@@ -53,6 +53,7 @@ public class SwerveFXS implements SwerveIO, Checkable {
   private SwerveDriveKinematics kinematics;
   private double fieldY = 0.0;
   private double fieldX = 0.0;
+  private boolean didZero = false;
 
   public SwerveFXS() {
 
@@ -65,7 +66,7 @@ public SwerveFXS() {
             .encoderOpposed(false);
     swerveModules = new FXSwerveModule[4];
     Translation2d[] wheelLocations = DriveConstants.getWheelLocationMeters();
-
+    didZero = true;
     for (int i = 0; i < 4; i++) {
       var azimuthFXS = new TalonFXS(i, "*");
       configuratorFXS = azimuthFXS.getConfigurator();
@@ -89,7 +90,8 @@ public SwerveFXS() {
               .wheelLocationMeters(wheelLocations[i])
               .closedLoopUnits(ClosedLoopUnits.VOLTAGE)
               .build();
-      swerveModules[i].loadAndSetAzimuthZeroReference();
+      boolean zeroCheck = swerveModules[i].zeroAndCheck();
+      didZero = zeroCheck && didZero;
     }
 
     pigeon = new SF_PIGEON2(DriveConstants.kPigeonCanID, "*");
@@ -120,6 +122,15 @@ public String getName() {
     return "Swerve";
   }
 
+  @Override
+  public void zeroModules() {
+    didZero = true;
+    for (int i = 0; i < 4; i++) {
+      boolean zeroCheck = swerveModules[i].zeroAndCheck();
+      didZero = zeroCheck && didZero;
+    }
+  }
+
   private SwerveModule[] getSwerveModules() {
     return swerveDrive.getSwerveModules();
   }
@@ -284,6 +295,7 @@ public void updateInputs(SwerveIOInputs inputs) {
     inputs.fieldRelSpeed = getFieldRelSpeed(inputs.robotRelSpeed);
     inputs.fieldY = fieldY;
     inputs.fieldX = fieldX;
+    inputs.didZero = didZero;
   }
 
   @Override

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

@@ -41,6 +41,7 @@ public static class SwerveIOInputs {
     public double[] azimuthCurrent = {0, 0, 0, 0};
     public double avgDriveCurrent = 0;
     public double avgRearDriveVel = 0;
+    public boolean didZero = false;
   }
 
   private SwerveModule[] getSwerveModules() {
@@ -101,4 +102,6 @@ public default void configDriveCurrents(CurrentLimitsConfigs config) {}
   public default BooleanSupplier getAzimuth1FwdLimitSwitch() {
     return () -> false;
   }
+
+  public default void zeroModules() {}
 }

vendordeps/thirdcoast.json

@@ -1,7 +1,7 @@
 {
     "fileName": "thirdcoast.json",
     "name": "StrykeForce-ThirdCoast",
-    "version": "25.0.6",
+    "version": "25.0.7",
     "uuid": "13c4f4b5-a1c0-4670-8f8d-b7b03628c0d3",
     "frcYear": "2025",
     "mavenUrls": [
@@ -12,7 +12,7 @@
         {
             "groupId": "org.strykeforce",
             "artifactId": "thirdcoast",
-            "version": "25.0.6"
+            "version": "25.0.7"
         }
     ],
     "jniDependencies": [],