From 345aa6aa0ed2cf99a3b4b391b3702777a5ece78d Mon Sep 17 00:00:00 2001 From: WesleyWong-972 Date: Mon, 23 Mar 2026 15:31:13 -0700 Subject: [PATCH] supposed fix? --- .../util/SwerveStuff/SwerveSetpointGenerator.java | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) 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); -- 2.39.5