diff --git a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java index a94fc65..88431a5 100644 --- a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java @@ -327,7 +327,7 @@ public void setDesiredState(ChassisSpeeds speeds, boolean fieldRelative) { SmartDashboard.putNumber("Spin", speeds.omegaRadiansPerSecond); if (fieldRelative) - speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getHeading().plus(Rotation2d.fromDegrees(180))); + speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getHeading()); speeds = ChassisSpeeds.discretize(speeds, TimedRobot.kDefaultPeriod); SwerveModuleState[] states = kinematics.toSwerveModuleStates(speeds);