From: WesleyWong-972 <wesleycwong@gmail.com>
Date: Mon, 23 Mar 2026 22:31:13 +0000 (-0700)
Subject: supposed fix?
X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=345aa6aa0ed2cf99a3b4b391b3702777a5ece78d;p=FRC2026.git

supposed fix?
---

diff --git a/src/main/java/frc/robot/util/SwerveStuff/SwerveSetpointGenerator.java b/src/main/java/frc/robot/util/SwerveStuff/SwerveSetpointGenerator.java
index 2c7dd20..b728857 100644
--- a/src/main/java/frc/robot/util/SwerveStuff/SwerveSetpointGenerator.java
+++ b/src/main/java/frc/robot/util/SwerveStuff/SwerveSetpointGenerator.java
@@ -462,11 +462,17 @@ public class SwerveSetpointGenerator {
       }
     }
 
+    double deltaTheta = prevSetpoint.chassisSpeeds().omegaRadiansPerSecond * dt;
+    var asTransform = new Translation2d(prevSetpoint.chassisSpeeds().vxMetersPerSecond, prevSetpoint.chassisSpeeds().vyMetersPerSecond);
+    asTransform = asTransform.rotateBy(new Rotation2d(deltaTheta));
+    var transformedPrevSpeeds = new ChassisSpeeds(asTransform.getX(), asTransform.getY(), prevSetpoint.chassisSpeeds().omegaRadiansPerSecond);
+
     ChassisSpeeds retSpeeds =
         new ChassisSpeeds(
-            prevSetpoint.chassisSpeeds().vxMetersPerSecond + min_s * dx,
-            prevSetpoint.chassisSpeeds().vyMetersPerSecond + min_s * dy,
-            prevSetpoint.chassisSpeeds().omegaRadiansPerSecond + min_s * dtheta);
+            transformedPrevSpeeds.vxMetersPerSecond + min_s * dx,
+            transformedPrevSpeeds.vyMetersPerSecond + min_s * dy,
+            transformedPrevSpeeds.omegaRadiansPerSecond + min_s * dtheta);
+
     var retStates = kinematics.toSwerveModuleStates(retSpeeds);
     for (int i = 0; i < modules.length; ++i) {
       final var maybeOverride = overrideSteering.get(i);