[wpimath] Add optimize() to SwerveModuleState

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

@@ -32,7 +32,12 @@ frc::SwerveModuleState SwerveModule::GetState() const {
           frc::Rotation2d(units::radian_t(m_turningEncoder.Get()))};
 }
 
-void SwerveModule::SetDesiredState(const frc::SwerveModuleState& state) {
+void SwerveModule::SetDesiredState(
+    const frc::SwerveModuleState& referenceState) {
+  // Optimize the reference state to avoid spinning further than 90 degrees
+  auto state = frc::SwerveModuleState::Optimize(
+      referenceState, frc::Rotation2d{units::radian_t(m_turningEncoder.Get())});
+
   // Calculate the drive output from the drive PID controller.
   const auto driveOutput = m_drivePIDController.Calculate(
       m_driveEncoder.GetRate(), state.speed.to<double>());

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

@@ -79,9 +79,13 @@ public SwerveModuleState getState() {
   /**
    * Sets the desired state for the module.
    *
-   * @param state Desired state with speed and angle.
+   * @param desiredState Desired state with speed and angle.
    */
-  public void setDesiredState(SwerveModuleState state) {
+  public void setDesiredState(SwerveModuleState desiredState) {
+    // Optimize the reference state to avoid spinning further than 90 degrees
+    SwerveModuleState state =
+        SwerveModuleState.optimize(desiredState, new Rotation2d(m_turningEncoder.get()));
+
     // Calculate the drive output from the drive PID controller.
     final double driveOutput =
         m_drivePIDController.calculate(m_driveEncoder.getRate(), state.speedMetersPerSecond);

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

@@ -48,4 +48,23 @@ public String toString() {
     return String.format(
         "SwerveModuleState(Speed: %.2f m/s, Angle: %s)", speedMetersPerSecond, angle);
   }
+
+  /**
+   * Optimize a desired state by deciding weather to reverse the direction the wheel spins in order
+   * to minimize travel time. This means that the furthest a wheel will ever rotate is 90 degrees,
+   * if used in conjunction with the PIDController class' continuous input functionality.
+   *
+   * @param desiredState The desired state.
+   * @param currentAngle The current module angle.
+   */
+  public static SwerveModuleState optimize(
+      SwerveModuleState desiredState, Rotation2d currentAngle) {
+    var delta = desiredState.angle.minus(currentAngle);
+    if (Math.abs(delta.getDegrees()) > 90.0) {
+      return new SwerveModuleState(
+          desiredState.speedMetersPerSecond * -1.0,
+          desiredState.angle.rotateBy(Rotation2d.fromDegrees(180.0)));
+    }
+    return new SwerveModuleState(desiredState.speedMetersPerSecond, desiredState.angle);
+  }
 }

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

@@ -5,6 +5,8 @@
 #pragma once
 
 #include "frc/geometry/Rotation2d.h"
+#include "units/angle.h"
+#include "units/math.h"
 #include "units/velocity.h"
 
 namespace frc {
@@ -21,5 +23,24 @@ struct SwerveModuleState {
    * Angle of the module.
    */
   Rotation2d angle;
+
+  /**
+   * Optimize a desired state by deciding weather to reverse the direction the
+   * wheel spins in order to minimize travel time. This means that the furthest
+   * a wheel will ever rotate is 90 degrees, if used in conjunction with the
+   * PIDController class' continuous input functionality.
+   * 
+   * @param desiredState The desired state.
+   * @param currentAngle The current module angle.
+   */
+  static SwerveModuleState Optimize(const SwerveModuleState& desiredState,
+                                    const Rotation2d& currentAngle) {
+    auto delta = desiredState.angle - currentAngle;
+    if (units::math::abs(delta.Degrees()) > 90_deg) {
+      return {desiredState.speed * -1.0,
+              desiredState.angle + Rotation2d{180_deg}};
+    }
+    return {desiredState.speed, desiredState.angle};
+  }
 };
 }  // namespace frc