Add module position sanity checks

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();

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);

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();