From 33638fddfe922c99701237aef04983c9a987985e Mon Sep 17 00:00:00 2001
From: Abigail-27 <ilovecats333meow@gmail.com>
Date: Wed, 8 Jan 2025 17:46:24 -0500
Subject: [PATCH] xstance created

---
 .../robot/subsystems/swerve/SwerveDrive.java  | 28 ++++++++++++++++++-
 .../robot/subsystems/swerve/SwerveModule.java |  7 +++--
 .../swerve/module/ModuleInterface.java        |  2 +-
 .../swerve/module/PhysicalModule.java         |  4 +--
 4 files changed, 35 insertions(+), 6 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java
index d743a85..a2c1960 100644
--- a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java
+++ b/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.
diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java
index f992ef8..075c556 100644
--- a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java
+++ b/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. */
diff --git a/src/main/java/frc/robot/subsystems/swerve/module/ModuleInterface.java b/src/main/java/frc/robot/subsystems/swerve/module/ModuleInterface.java
index fe08416..7305d2c 100644
--- a/src/main/java/frc/robot/subsystems/swerve/module/ModuleInterface.java
+++ b/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;
   }
diff --git a/src/main/java/frc/robot/subsystems/swerve/module/PhysicalModule.java b/src/main/java/frc/robot/subsystems/swerve/module/PhysicalModule.java
index fbdf6c7..96ae654 100644
--- a/src/main/java/frc/robot/subsystems/swerve/module/PhysicalModule.java
+++ b/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