refactor(swerve): Use simpler optimization.

src/main/java/frc/robot/swerve/SwerveModuleIOCustom.java

@@ -3,7 +3,6 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
-import edu.wpi.first.math.util.Units;
 import frc.robot.swerve.AzimuthEncoderIO.AzimuthEncoderIOValues;
 import frc.robot.swerve.DriveMotorIO.DriveMotorIOValues;
 import frc.robot.swerve.SteerMotorIO.SteerMotorIOValues;
@@ -56,7 +55,7 @@ public SwerveModuleIOCustom(SwerveModuleConfig config) {
   @Override
   public void setSetpoint(SwerveModuleState setpoint, boolean lazy) {
     if (lazy) {
-      setpoint = optimize(setpoint);
+      setpoint = optimize(setpoint, getState());
     }
 
     steerMotor.setSetpoint(setpoint.angle.getRotations());
@@ -69,23 +68,11 @@ public void setSetpoint(SwerveModuleState setpoint, boolean lazy) {
    * Optimizes a swerve module's setpoint.
    *
    * @param setpoint the setpoint to optimize.
+   * @param state the state of the module.
    * @return the optimized setpoint.
    */
-  private SwerveModuleState optimize(SwerveModuleState setpoint) {
-    setpoint =
-        SwerveModuleState.optimize(
-            setpoint, Rotation2d.fromRotations(steerMotorValues.positionRotations));
-
-    final double kDejitterThreshold = Units.inchesToMeters(4);
-
-    if (Math.abs(setpoint.speedMetersPerSecond) < kDejitterThreshold) {
-      setpoint.angle = getState().angle;
-    }
-
-    // https://github.com/Mechanical-Advantage/RobotCode2023/blob/bf960378bca7fe3f32c46d3d529925d960d1ff37/src/main/java/org/littletonrobotics/frc2023/subsystems/drive/Module.java#L117
-    setpoint.speedMetersPerSecond *= setpoint.angle.minus(getState().angle).getCos();
-
-    return setpoint;
+  private SwerveModuleState optimize(SwerveModuleState setpoint, SwerveModuleState state) {
+    return SwerveModuleState.optimize(setpoint, state.angle);
   }
 
   @Override