From 66049f135700fd89a86f420337f8f4e851812c30 Mon Sep 17 00:00:00 2001
From: Bradley Bickford <bradleybickford@gmail.com>
Date: Sat, 31 Jan 2026 17:03:35 -0500
Subject: [PATCH] FINALLY FIXED SWERVE

---
 src/main/java/frc/robot/RobotContainer.java   | 12 ++++++----
 .../frc/robot/constants/ModuleConstants.java  |  4 +++-
 .../java/frc/robot/subsystems/Drivetrain.java | 24 +++++++++----------
 3 files changed, 23 insertions(+), 17 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 1c41ea8..94aa618 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -50,10 +50,14 @@ public class RobotContainer {
       )
     );
 
-    driver.x().whileTrue(drivetrain.runFrontLeft());
-    driver.y().whileTrue(drivetrain.runFrontRight());
-    driver.a().whileTrue(drivetrain.runRearLeft());
-    driver.b().whileTrue(drivetrain.runRearRight());
+    driver.start().and(driver.x()).whileTrue(drivetrain.runFrontLeft(1, 0));
+    driver.start().and(driver.y()).whileTrue(drivetrain.runFrontRight(1, 0));
+    driver.start().and(driver.a()).whileTrue(drivetrain.runRearLeft(1, 0));
+    driver.start().and(driver.b()).whileTrue(drivetrain.runRearRight(1, 0));
+    driver.start().negate().and(driver.x()).whileTrue(drivetrain.runFrontLeft(0, 45));
+    driver.start().negate().and(driver.y()).whileTrue(drivetrain.runFrontRight(0, 45));
+    driver.start().negate().and(driver.a()).whileTrue(drivetrain.runRearLeft(0, 45));
+    driver.start().negate().and(driver.b()).whileTrue(drivetrain.runRearRight(0, 45));
     driver.rightBumper().whileTrue(drivetrain.setX());
 
     //drivetrain.setDefaultCommand(drivetrain.disableOutputs());
diff --git a/src/main/java/frc/robot/constants/ModuleConstants.java b/src/main/java/frc/robot/constants/ModuleConstants.java
index 1972ca4..c91c941 100644
--- a/src/main/java/frc/robot/constants/ModuleConstants.java
+++ b/src/main/java/frc/robot/constants/ModuleConstants.java
@@ -90,7 +90,8 @@ public class ModuleConstants {
 
         turningConfig
             .idleMode(kTurnIdleMode)
-            .smartCurrentLimit(kTurnMotorCurrentLimit);
+            .smartCurrentLimit(kTurnMotorCurrentLimit)
+            .inverted(true);
         turningConfig.encoder
             //.inverted(true)
             .positionConversionFactor(kTurningFactor)
@@ -101,5 +102,6 @@ public class ModuleConstants {
             .outputRange(-1, 1)
             .positionWrappingEnabled(true)
             .positionWrappingInputRange(0, 2 * Math.PI);
+            
     }
 }
diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index d880e5d..3281da4 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -134,32 +134,32 @@ public class Drivetrain extends SubsystemBase {
         Logger.recordOutput("Drivetrain/Heading", getHeading());
     }
 
-    public Command runFrontLeft() {
+    public Command runFrontLeft(double staticSpeed, double staticAngleDegrees) {
         return run(() -> {
             frontLeft.setDesiredState(new SwerveModuleState(
-                1, 
-                new Rotation2d()));
+                staticSpeed, 
+                Rotation2d.fromDegrees(staticAngleDegrees)));
         });
     }
-    public Command runFrontRight() {
+    public Command runFrontRight(double staticSpeed, double staticAngleDegrees) {
         return run(() -> {
             frontRight.setDesiredState(new SwerveModuleState(
-                1, 
-                new Rotation2d()));
+                staticSpeed, 
+                Rotation2d.fromDegrees(staticAngleDegrees)));
         });
     }
-    public Command runRearLeft() {
+    public Command runRearLeft(double staticSpeed, double staticAngleDegrees) {
         return run(() -> {
             rearLeft.setDesiredState(new SwerveModuleState(
-                1, 
-                new Rotation2d()));
+                staticSpeed, 
+                Rotation2d.fromDegrees(staticAngleDegrees)));
         });
     }
-    public Command runRearRight() {
+    public Command runRearRight(double staticSpeed, double staticAngleDegrees) {
         return run(() -> {
             rearRight.setDesiredState(new SwerveModuleState(
-                1, 
-                new Rotation2d()));
+                staticSpeed, 
+                Rotation2d.fromDegrees(staticAngleDegrees)));
         });
     }