[wpimath] Add cosineScale method to SwerveModuleState and instance op…

…timize (wpilibsuite#7114)

wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp

@@ -46,29 +46,29 @@ frc::SwerveModulePosition SwerveModule::GetPosition() const {
           units::radian_t{m_turningEncoder.GetDistance()}};
 }
 
-void SwerveModule::SetDesiredState(
-    const frc::SwerveModuleState& referenceState) {
+void SwerveModule::SetDesiredState(frc::SwerveModuleState& referenceState) {
   frc::Rotation2d encoderRotation{
       units::radian_t{m_turningEncoder.GetDistance()}};
 
   // Optimize the reference state to avoid spinning further than 90 degrees
-  auto state =
-      frc::SwerveModuleState::Optimize(referenceState, encoderRotation);
+  referenceState.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.speed *= (state.angle - encoderRotation).Cos();
+  referenceState.CosineScale(encoderRotation);
 
   // Calculate the drive output from the drive PID controller.
   const auto driveOutput = m_drivePIDController.Calculate(
-      m_driveEncoder.GetRate(), state.speed.value());
+      m_driveEncoder.GetRate(), referenceState.speed.value());
 
-  const auto driveFeedforward = m_driveFeedforward.Calculate(state.speed);
+  const auto driveFeedforward =
+      m_driveFeedforward.Calculate(referenceState.speed);
 
   // Calculate the turning motor output from the turning PID controller.
   const auto turnOutput = m_turningPIDController.Calculate(
-      units::radian_t{m_turningEncoder.GetDistance()}, state.angle.Radians());
+      units::radian_t{m_turningEncoder.GetDistance()},
+      referenceState.angle.Radians());
 
   const auto turnFeedforward = m_turnFeedforward.Calculate(
       m_turningPIDController.GetSetpoint().velocity);

wpilibcExamples/src/main/cpp/examples/SwerveBot/include/SwerveModule.h

@@ -25,7 +25,7 @@ class SwerveModule {
                int turningEncoderChannelA, int turningEncoderChannelB);
   frc::SwerveModuleState GetState() const;
   frc::SwerveModulePosition GetPosition() const;
-  void SetDesiredState(const frc::SwerveModuleState& state);
+  void SetDesiredState(frc::SwerveModuleState& state);
 
  private:
   static constexpr double kWheelRadius = 0.0508;

wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/SwerveModule.cpp

@@ -53,27 +53,26 @@ frc::SwerveModulePosition SwerveModule::GetPosition() {
           units::radian_t{m_turningEncoder.GetDistance()}};
 }
 
-void SwerveModule::SetDesiredState(
-    const frc::SwerveModuleState& referenceState) {
+void SwerveModule::SetDesiredState(frc::SwerveModuleState& referenceState) {
   frc::Rotation2d encoderRotation{
       units::radian_t{m_turningEncoder.GetDistance()}};
 
   // Optimize the reference state to avoid spinning further than 90 degrees
-  auto state =
-      frc::SwerveModuleState::Optimize(referenceState, encoderRotation);
+  referenceState.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.speed *= (state.angle - encoderRotation).Cos();
+  referenceState.CosineScale(encoderRotation);
 
   // Calculate the drive output from the drive PID controller.
   const auto driveOutput = m_drivePIDController.Calculate(
-      m_driveEncoder.GetRate(), state.speed.value());
+      m_driveEncoder.GetRate(), referenceState.speed.value());
 
   // Calculate the turning motor output from the turning PID controller.
   auto turnOutput = m_turningPIDController.Calculate(
-      units::radian_t{m_turningEncoder.GetDistance()}, state.angle.Radians());
+      units::radian_t{m_turningEncoder.GetDistance()},
+      referenceState.angle.Radians());
 
   // Set the motor outputs.
   m_driveMotor.Set(driveOutput);

wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/SwerveModule.h

@@ -27,7 +27,7 @@ class SwerveModule {
 
   frc::SwerveModulePosition GetPosition();
 
-  void SetDesiredState(const frc::SwerveModuleState& state);
+  void SetDesiredState(frc::SwerveModuleState& state);
 
   void ResetEncoders();
 

wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/SwerveModule.cpp

@@ -46,29 +46,29 @@ frc::SwerveModulePosition SwerveModule::GetPosition() const {
           units::radian_t{m_turningEncoder.GetDistance()}};
 }
 
-void SwerveModule::SetDesiredState(
-    const frc::SwerveModuleState& referenceState) {
+void SwerveModule::SetDesiredState(frc::SwerveModuleState& referenceState) {
   frc::Rotation2d encoderRotation{
       units::radian_t{m_turningEncoder.GetDistance()}};
 
   // Optimize the reference state to avoid spinning further than 90 degrees
-  auto state =
-      frc::SwerveModuleState::Optimize(referenceState, encoderRotation);
+  referenceState.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.speed *= (state.angle - encoderRotation).Cos();
+  referenceState.CosineScale(encoderRotation);
 
   // Calculate the drive output from the drive PID controller.
   const auto driveOutput = m_drivePIDController.Calculate(
-      m_driveEncoder.GetRate(), state.speed.value());
+      m_driveEncoder.GetRate(), referenceState.speed.value());
 
-  const auto driveFeedforward = m_driveFeedforward.Calculate(state.speed);
+  const auto driveFeedforward =
+      m_driveFeedforward.Calculate(referenceState.speed);
 
   // Calculate the turning motor output from the turning PID controller.
   const auto turnOutput = m_turningPIDController.Calculate(
-      units::radian_t{m_turningEncoder.GetDistance()}, state.angle.Radians());
+      units::radian_t{m_turningEncoder.GetDistance()},
+      referenceState.angle.Radians());
 
   const auto turnFeedforward = m_turnFeedforward.Calculate(
       m_turningPIDController.GetSetpoint().velocity);

wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/SwerveModule.h

@@ -25,7 +25,7 @@ class SwerveModule {
                int turningEncoderChannelA, int turningEncoderChannelB);
   frc::SwerveModuleState GetState() const;
   frc::SwerveModulePosition GetPosition() const;
-  void SetDesiredState(const frc::SwerveModuleState& state);
+  void SetDesiredState(frc::SwerveModuleState& state);
 
  private:
   static constexpr auto kWheelRadius = 0.0508_m;

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

@@ -115,24 +115,27 @@ 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,21 @@ 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,26 @@ 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

@@ -83,6 +83,21 @@ public String toString() {
         "SwerveModuleState(Speed: %.2f m/s, Angle: %s)", speedMetersPerSecond, angle);
   }
 
+  /**
+   * Minimize the change in heading this 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);
+    }
+  }
+
   /**
    * 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
@@ -91,7 +106,9 @@ public String toString() {
    * @param desiredState The desired state.
    * @param currentAngle The current module angle.
    * @return Optimized swerve module state.
+   * @deprecated Use the instance method instead.
    */
+  @Deprecated
   public static SwerveModuleState optimize(
       SwerveModuleState desiredState, Rotation2d currentAngle) {
     var delta = desiredState.angle.minus(currentAngle);
@@ -102,4 +119,15 @@ public static SwerveModuleState optimize(
       return new SwerveModuleState(desiredState.speedMetersPerSecond, desiredState.angle);
     }
   }
+
+  /**
+   * Scales 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();
+  }
 }

wpimath/src/main/native/include/frc/kinematics/SwerveModuleState.h

@@ -34,6 +34,22 @@ struct WPILIB_DLLEXPORT SwerveModuleState {
    */
   bool operator==(const SwerveModuleState& other) const;
 
+  /**
+   * Minimize the change in the heading this 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.
+   */
+  void Optimize(const Rotation2d& currentAngle) {
+    auto delta = angle - currentAngle;
+    if (units::math::abs(delta.Degrees()) > 90_deg) {
+      speed *= -1;
+      angle = angle + Rotation2d{180_deg};
+    }
+  }
+
   /**
    * Minimize the change in heading the desired swerve module state would
    * require by potentially reversing the direction the wheel spins. If this is
@@ -43,8 +59,20 @@ struct WPILIB_DLLEXPORT SwerveModuleState {
    * @param desiredState The desired state.
    * @param currentAngle The current module angle.
    */
+  [[deprecated("Use instance method instead.")]]
   static SwerveModuleState Optimize(const SwerveModuleState& desiredState,
                                     const Rotation2d& currentAngle);
+
+  /**
+   * Scales 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.
+   */
+  void CosineScale(const Rotation2d& currentAngle) {
+    speed *= (angle - currentAngle).Cos();
+  }
 };
 }  // namespace frc