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 2c7dd20029a73e7ec2e07bb553b35fd531018fe4..b728857074393671e6b3c583a3d45f54b4bc7a50 100644 (file)
--- 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);