[wpimath] Add cosineScale and instance optimize to SwerveModuleState

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java

@@ -115,24 +115,24 @@ public void setDesiredState(SwerveModuleState desiredState) {
     var encoderRotation = new Rotation2d(m_turningEncoder.getDistance());
 
     // Optimize the reference state to avoid spinning further than 90 degrees
-    SwerveModuleState state = SwerveModuleState.optimize(desiredState, encoderRotation);
+    desiredState.optimize(encoderRotation);
 
     // Scale speed by cosine of angle error. This scales down movement perpendicular to the desired
     // direction of travel that can occur when modules change directions. This results in smoother
     // driving.
-    state.speedMetersPerSecond *= state.angle.minus(encoderRotation).getCos();
+    desiredState.cosineScale(encoderRotation);
 
     // Calculate the drive output from the drive PID controller.
 
     final double driveOutput =
-        m_drivePIDController.calculate(m_driveEncoder.getRate(), state.speedMetersPerSecond);
+        m_drivePIDController.calculate(m_driveEncoder.getRate(), desiredState.speedMetersPerSecond);
 
     final double driveFeedforward =
-        m_driveFeedforward.calculate(MetersPerSecond.of(state.speedMetersPerSecond)).in(Volts);
+        m_driveFeedforward.calculate(MetersPerSecond.of(desiredState.speedMetersPerSecond)).in(Volts);
 
     // Calculate the turning motor output from the turning PID controller.
     final double turnOutput =
-        m_turningPIDController.calculate(m_turningEncoder.getDistance(), state.angle.getRadians());
+        m_turningPIDController.calculate(m_turningEncoder.getDistance(), desiredState.angle.getRadians());
 
     final double turnFeedforward =
         m_turnFeedforward

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/SwerveModule.java

@@ -108,20 +108,20 @@ public void setDesiredState(SwerveModuleState desiredState) {
     var encoderRotation = new Rotation2d(m_turningEncoder.getDistance());
 
     // Optimize the reference state to avoid spinning further than 90 degrees
-    SwerveModuleState state = SwerveModuleState.optimize(desiredState, encoderRotation);
+    desiredState.optimize(encoderRotation);
 
     // Scale speed by cosine of angle error. This scales down movement perpendicular to the desired
     // direction of travel that can occur when modules change directions. This results in smoother
     // driving.
-    state.speedMetersPerSecond *= state.angle.minus(encoderRotation).getCos();
+    desiredState.cosineScale(encoderRotation);
 
     // Calculate the drive output from the drive PID controller.
     final double driveOutput =
-        m_drivePIDController.calculate(m_driveEncoder.getRate(), state.speedMetersPerSecond);
+        m_drivePIDController.calculate(m_driveEncoder.getRate(), desiredState.speedMetersPerSecond);
 
     // Calculate the turning motor output from the turning PID controller.
     final double turnOutput =
-        m_turningPIDController.calculate(m_turningEncoder.getDistance(), state.angle.getRadians());
+        m_turningPIDController.calculate(m_turningEncoder.getDistance(), desiredState.angle.getRadians());
 
     // Calculate the turning motor output from the turning PID controller.
     m_driveMotor.set(driveOutput);

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java

@@ -115,23 +115,23 @@ public void setDesiredState(SwerveModuleState desiredState) {
     var encoderRotation = new Rotation2d(m_turningEncoder.getDistance());
 
     // Optimize the reference state to avoid spinning further than 90 degrees
-    SwerveModuleState state = SwerveModuleState.optimize(desiredState, encoderRotation);
+    desiredState.optimize(encoderRotation);
 
     // Scale speed by cosine of angle error. This scales down movement perpendicular to the desired
     // direction of travel that can occur when modules change directions. This results in smoother
     // driving.
-    state.speedMetersPerSecond *= state.angle.minus(encoderRotation).getCos();
+    desiredState.cosineScale(encoderRotation);
 
     // Calculate the drive output from the drive PID controller.
     final double driveOutput =
-        m_drivePIDController.calculate(m_driveEncoder.getRate(), state.speedMetersPerSecond);
+        m_drivePIDController.calculate(m_driveEncoder.getRate(), desiredState.speedMetersPerSecond);
 
     final double driveFeedforward =
-        m_driveFeedforward.calculate(MetersPerSecond.of(state.speedMetersPerSecond)).in(Volts);
+        m_driveFeedforward.calculate(MetersPerSecond.of(desiredState.speedMetersPerSecond)).in(Volts);
 
     // Calculate the turning motor output from the turning PID controller.
     final double turnOutput =
-        m_turningPIDController.calculate(m_turningEncoder.getDistance(), state.angle.getRadians());
+        m_turningPIDController.calculate(m_turningEncoder.getDistance(), desiredState.angle.getRadians());
 
     final double turnFeedforward =
         m_turnFeedforward

wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java

@@ -30,13 +30,14 @@ public class SwerveModuleState
   public static final SwerveModuleStateStruct struct = new SwerveModuleStateStruct();
 
   /** Constructs a SwerveModuleState with zeros for speed and angle. */
-  public SwerveModuleState() {}
+  public SwerveModuleState() {
+  }
 
   /**
    * Constructs a SwerveModuleState.
    *
    * @param speedMetersPerSecond The speed of the wheel of the module.
-   * @param angle The angle of the module.
+   * @param angle                The angle of the module.
    */
   public SwerveModuleState(double speedMetersPerSecond, Rotation2d angle) {
     this.speedMetersPerSecond = speedMetersPerSecond;
@@ -66,7 +67,8 @@ public int hashCode() {
   }
 
   /**
-   * Compares two swerve module states. One swerve module is "greater" than the other if its speed
+   * Compares two swerve module states. One swerve module is "greater" than the
+   * other if its speed
    * is higher than the other.
    *
    * @param other The other swerve module.
@@ -84,14 +86,50 @@ public String toString() {
   }
 
   /**
-   * Minimize the change in heading the desired swerve module state would require by potentially
-   * reversing the direction the wheel spins. If this is used with the PIDController class's
-   * continuous input functionality, the furthest a wheel will ever rotate is 90 degrees.
+   * Minimize the change in heading the desired swerve module state would require
+   * by potentially
+   * reversing the direction the wheel spins. If this is used with the
+   * PIDController class's
+   * continuous input functionality, the furthest a wheel will ever rotate is 90
+   * degrees.
+   *
+   * @param currentAngle The current module angle.
+   */
+  public void optimize(Rotation2d currentAngle) {
+    var delta = angle.minus(currentAngle);
+    if (Math.abs(delta.getDegrees()) > 90.0) {
+      speedMetersPerSecond *= -1;
+      angle = angle.rotateBy(Rotation2d.kPi);
+    }
+  }
+
+  /**
+   * Scale speed by cosine of angle error. This scales down movement perpendicular
+   * to the desired
+   * direction of travel that can occur when modules change directions. This
+   * results in smoother
+   * driving.
+   *
+   * @param currentAngle The current module angle.
+   */
+  public void cosineScale(Rotation2d currentAngle) {
+    speedMetersPerSecond *= angle.minus(currentAngle).getCos();
+  }
+
+  /**
+   * Minimize the change in heading the desired swerve module state would require
+   * by potentially
+   * reversing the direction the wheel spins. If this is used with the
+   * PIDController class's
+   * continuous input functionality, the furthest a wheel will ever rotate is 90
+   * degrees.
    *
    * @param desiredState The desired state.
    * @param currentAngle The current module angle.
    * @return Optimized swerve module state.
+   * @deprecated use the optimize instance method instead.
    */
+  @Deprecated
   public static SwerveModuleState optimize(
       SwerveModuleState desiredState, Rotation2d currentAngle) {
     var delta = desiredState.angle.minus(currentAngle);