xstance created

src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java

@@ -278,8 +278,34 @@ public Rotation2d getOdometryAllianceRelativeRotation2d() {
   public void setModuleStates(SwerveModuleState[] desiredStates) {
     for (int i = 0; i < 4; i++) {
       swerveModules[i].runSetPoint((desiredStates[i]));
-    }
   }
+ }
+
+ private boolean isMoving; // Tracks if the robot is moving
+ private long lastMovementTime = System.currentTimeMillis(); // Time of the last movement
+ private static final long INACTIVITY_THRESHOLD = 3000; // 3 seconds in milliseconds
+
+ // Check if the robot has been inactive for 3 seconds
+ public boolean threesecsinactive() {
+     if (!isMoving && System.currentTimeMillis() - lastMovementTime >= INACTIVITY_THRESHOLD) {
+         return true; // 3 seconds of inactivity
+     }
+     return false; // Still moving or not enough time passed
+ }
+
+ public void setXStance(SwerveModuleState[] xState) {
+    isMoving = false;
+     if (threesecsinactive()) { // Only lock if inactive for 3 seconds
+         Rotation2d[] swerveHeadings = new Rotation2d[swerveModules.length];
+         for (int i = 0; i < swerveHeadings.length; i++) {
+             swerveHeadings[i] = swerveModules[i].getPosition().angle;
+         }
+         DriveConstants.DRIVE_KINEMATICS.resetHeadings(swerveHeadings);
+         for (int i = 0; i < swerveModules.length; i++) {
+             swerveModules[i].stopModule();
+         }
+     }
+ }
 
   /**
    * Updates the pose estimator with the pose calculated from the swerve modules.

src/main/java/frc/robot/subsystems/swerve/SwerveModule.java

@@ -68,6 +68,9 @@ public void runSetPoint(SwerveModuleState state) {
     Logger.recordOutput("Drive/desired turn angle", state.angle.getRotations());
   }
 
+  public void stopModule() {
+    io.stopModule();
+  }
   /** Returns the current turn angle of the module. */
   public Rotation2d getTurnRotation() {
     return inputs.turnAbsolutePosition;
@@ -77,8 +80,8 @@ public double getTurnVelocity() {
     return inputs.turnVelocity;
   }
 
-  public double xthing() {
-    return inputs.Angle
+  public void xthing(double desiredPositionDegrees) {
+    io.xthing(desiredPositionDegrees);
   }
 
   /** Returns the current drive position of the module in meters. */

src/main/java/frc/robot/subsystems/swerve/module/ModuleInterface.java

@@ -47,7 +47,7 @@ default void setTurnVoltage(Voltage voltage) {}
 
   default void stopModule() {}
 
-  default void xthing() {}
+  default void xthing(double desiredPositionDegrees) {}
   default double getTurnRotations() {
     return 0.0;
   }

src/main/java/frc/robot/subsystems/swerve/module/PhysicalModule.java

@@ -188,8 +188,8 @@ public double getTurnRotations() {
   }
 
   @Override
-  public void xthing() {
-    turnMotor.setPosition(null);
+  public void xthing(double desiredPositionDegrees) {
+    turnMotor.setControl(mmPositionRequest.withPosition(Degrees.of(desiredPositionDegrees)));
   }
 
   @Override