added cosineScale and instance optimize

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommandTest.java

@@ -139,4 +139,4 @@ void testReachesReference() {
                 getRobotPose().getRotation().getRadians(),
                 kAngularTolerance));
   }
-}
+}

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

@@ -120,7 +120,7 @@ public void setDesiredState(SwerveModuleState desiredState) {
     // 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();
+    state.cosineScale(encoderRotation);
 
     // Calculate the drive output from the drive PID controller.
 
@@ -142,4 +142,4 @@ public void setDesiredState(SwerveModuleState desiredState) {
     m_driveMotor.setVoltage(driveOutput + driveFeedforward);
     m_turningMotor.setVoltage(turnOutput + turnFeedforward);
   }
-}
+}

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

@@ -133,4 +133,4 @@ public void resetEncoders() {
     m_driveEncoder.reset();
     m_turningEncoder.reset();
   }
-}
+}

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

@@ -141,4 +141,4 @@ public void setDesiredState(SwerveModuleState desiredState) {
     m_driveMotor.setVoltage(driveOutput + driveFeedforward);
     m_turningMotor.setVoltage(turnOutput + turnFeedforward);
   }
-}
+}

wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java

@@ -95,4 +95,4 @@ public Pose2d updateWithTime(
 
     return super.updateWithTime(currentTimeSeconds, gyroAngle, wheelPositions);
   }
-}
+}

wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java

@@ -337,4 +337,4 @@ public Rotation2d interpolate(Rotation2d endValue, double t) {
 
   /** Rotation2d struct for serialization. */
   public static final Rotation2dStruct struct = new Rotation2dStruct();
-}
+}

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

@@ -444,4 +444,4 @@ public Translation2d[] getModules() {
   public static final SwerveDriveKinematicsStruct getStruct(int numModules) {
     return new SwerveDriveKinematicsStruct(numModules);
   }
-}
+}

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);
@@ -102,4 +140,4 @@ public static SwerveModuleState optimize(
       return new SwerveModuleState(desiredState.speedMetersPerSecond, desiredState.angle);
     }
   }
-}
+}

wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/SwerveModuleStateProto.java

@@ -37,4 +37,4 @@ public void pack(ProtobufSwerveModuleState msg, SwerveModuleState value) {
     msg.setSpeed(value.speedMetersPerSecond);
     Rotation2d.proto.pack(msg.getMutableAngle(), value.angle);
   }
-}
+}

wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/SwerveModuleStateStruct.java

@@ -47,4 +47,4 @@ public void pack(ByteBuffer bb, SwerveModuleState value) {
     bb.putDouble(value.speedMetersPerSecond);
     Rotation2d.struct.pack(bb, value.angle);
   }
-}
+}

wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java

@@ -422,4 +422,4 @@ void testSampleAt() {
     assertEquals(Optional.of(new Pose2d(1, 0.1, Rotation2d.kZero)), estimator.sampleAt(0.5));
     assertEquals(Optional.of(new Pose2d(2, 0.1, Rotation2d.kZero)), estimator.sampleAt(2.5));
   }
-}
+}

wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java

@@ -410,4 +410,4 @@ void testDesaturateNegativeSpeed() {
         () -> assertEquals(-1.0, arr[2].speedMetersPerSecond, kEpsilon),
         () -> assertEquals(-1.0, arr[3].speedMetersPerSecond, kEpsilon));
   }
-}
+}

wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometryTest.java

@@ -273,4 +273,4 @@ void testAccuracyFacingXAxis() {
         0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.06, "Incorrect mean error");
     assertEquals(0.0, maxError, 0.125, "Incorrect max error");
   }
-}
+}

wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveModuleStateTest.java

@@ -50,4 +50,4 @@ void testNoOptimize() {
         () -> assertEquals(-2.0, optimizedB.speedMetersPerSecond, kEpsilon),
         () -> assertEquals(-2.0, optimizedB.angle.getDegrees(), kEpsilon));
   }
-}
+}

wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/SwerveModuleStateProtoTest.java

@@ -23,4 +23,4 @@ void testRoundtrip() {
     assertEquals(DATA.speedMetersPerSecond, data.speedMetersPerSecond);
     assertEquals(DATA.angle, data.angle);
   }
-}
+}

wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/SwerveModuleStateStructTest.java

@@ -26,4 +26,4 @@ void testRoundtrip() {
     assertEquals(DATA.speedMetersPerSecond, data.speedMetersPerSecond);
     assertEquals(DATA.angle, data.angle);
   }
-}
+}