From c5df269a4978755d274ef117030058bdc99a4db6 Mon Sep 17 00:00:00 2001
From: Bradley Bickford <bradleybickford@gmail.com>
Date: Sat, 31 Jan 2026 13:54:12 -0500
Subject: [PATCH] Add module position sanity checks

---
 src/main/java/frc/robot/RobotContainer.java   |  5 ++++
 .../frc/robot/constants/ModuleConstants.java  |  2 +-
 .../java/frc/robot/subsystems/Drivetrain.java | 29 +++++++++++++++++++
 3 files changed, 35 insertions(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 6e54ed1..9a2ec32 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -50,6 +50,11 @@ public class RobotContainer {
       )
     );
 
+    driver.x().whileTrue(drivetrain.runFrontLeft());
+    driver.y().whileTrue(drivetrain.runFrontRight());
+    driver.a().whileTrue(drivetrain.runRearLeft());
+    driver.b().whileTrue(drivetrain.runRearRight());
+
     //drivetrain.setDefaultCommand(drivetrain.disableOutputs());
 
     configureShiftDisplay();
diff --git a/src/main/java/frc/robot/constants/ModuleConstants.java b/src/main/java/frc/robot/constants/ModuleConstants.java
index 1972ca4..706414e 100644
--- a/src/main/java/frc/robot/constants/ModuleConstants.java
+++ b/src/main/java/frc/robot/constants/ModuleConstants.java
@@ -49,7 +49,7 @@ public class ModuleConstants {
     public static final double kTurnI = 0;
     public static final double kTurnD = 0;
 
-    public static final boolean kIsEncoderInverted = false;
+    public static final boolean kIsEncoderInverted = true;
 
     // TODO How sensitive is too sensitive?
     public static final double kAutoResetPositionDeadband = Units.degreesToRadians(.25);
diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 7990836..d880e5d 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -134,6 +134,35 @@ public class Drivetrain extends SubsystemBase {
         Logger.recordOutput("Drivetrain/Heading", getHeading());
     }
 
+    public Command runFrontLeft() {
+        return run(() -> {
+            frontLeft.setDesiredState(new SwerveModuleState(
+                1, 
+                new Rotation2d()));
+        });
+    }
+    public Command runFrontRight() {
+        return run(() -> {
+            frontRight.setDesiredState(new SwerveModuleState(
+                1, 
+                new Rotation2d()));
+        });
+    }
+    public Command runRearLeft() {
+        return run(() -> {
+            rearLeft.setDesiredState(new SwerveModuleState(
+                1, 
+                new Rotation2d()));
+        });
+    }
+    public Command runRearRight() {
+        return run(() -> {
+            rearRight.setDesiredState(new SwerveModuleState(
+                1, 
+                new Rotation2d()));
+        });
+    }
+
     public Command disableOutputs() {
         return run(() -> {
             frontLeft.disableOutput();