Hopefully resolves wpilibsuite/allwpilib#7332. I want to try it out o…

…n a robot to see if there is a noticeable difference.
TechnoTitans · Nov 14, 2024 · e04d208 · e04d208
1 parent a1ff5e3
commit e04d208
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 19 deletions.
diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/constants/Constants.java
@@ -10,7 +10,7 @@
 import org.photonvision.PhotonPoseEstimator;
 
 public interface Constants {
-    RobotMode CURRENT_MODE = RobotMode.REAL;
+    RobotMode CURRENT_MODE = RobotMode.SIM;
     CompetitionType CURRENT_COMPETITION_TYPE = CompetitionType.COMPETITION;
     double LOOP_PERIOD_SECONDS = 0.02;
 

diff --git a/src/main/java/frc/robot/subsystems/drive/Swerve.java b/src/main/java/frc/robot/subsystems/drive/Swerve.java
@@ -459,21 +459,6 @@ public void drive(final SwerveModuleState[] states) {
         backRight.setDesiredState(states[3]);
     }
 
-    public void drive(final SwerveModuleState[] states, final ChassisSpeeds desiredSpeeds) {
-        SwerveDriveKinematics.desaturateWheelSpeeds(
-                states,
-                desiredSpeeds,
-                maxLinearVelocity,
-                maxLinearVelocity,
-                maxAngularVelocity
-        );
-
-        frontLeft.setDesiredState(states[0]);
-        frontRight.setDesiredState(states[1]);
-        backLeft.setDesiredState(states[2]);
-        backRight.setDesiredState(states[3]);
-    }
-
     public void drive(
             final double xSpeedMeterPerSec,
             final double ySpeedMetersPerSec,
@@ -500,12 +485,16 @@ public void drive(
     }
 
     public void drive(final ChassisSpeeds speeds) {
+        final SwerveModuleState[] moduleStates = kinematics.toSwerveModuleStates(speeds);
+
+        SwerveDriveKinematics.desaturateWheelSpeeds(moduleStates, maxLinearVelocity);
+
         final ChassisSpeeds correctedSpeeds = ChassisSpeeds.discretize(
-                speeds,
-                4 * Constants.LOOP_PERIOD_SECONDS
+                kinematics.toChassisSpeeds(moduleStates),
+                Constants.LOOP_PERIOD_SECONDS
         );
 
-        drive(kinematics.toSwerveModuleStates(correctedSpeeds), speeds);
+        drive(kinematics.toSwerveModuleStates(correctedSpeeds));
     }
 
     public Command teleopDriveCommand(