first draft mutRotation2d and mutable SwerveModuleState

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

@@ -121,8 +121,8 @@ void testReachesReference() {
       m_angle = trajectory.sample(m_timer.get()).poseMeters.getRotation();
 
       for (int i = 0; i < m_modulePositions.length; i++) {
-        m_modulePositions[i].distanceMeters += m_moduleStates[i].speedMetersPerSecond * 0.005;
-        m_modulePositions[i].angle = m_moduleStates[i].angle;
+        m_modulePositions[i].distanceMeters += m_moduleStates[i].getSpeedMetersPerSecond() * 0.005;
+        m_modulePositions[i].angle = m_moduleStates[i].getAngle();
       }
 
       SimHooks.stepTiming(0.005);

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

@@ -11,6 +11,7 @@
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.geometry.MutRotation2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
@@ -32,6 +33,10 @@ public class SwerveModule {
   private final Encoder m_driveEncoder;
   private final Encoder m_turningEncoder;
 
+  private final MutRotation2d m_moduleAngle = new MutRotation2d();
+  private final SwerveModuleState m_moduleState = new SwerveModuleState();
+  private final MutRotation2d m_encoderRotation = new MutRotation2d();
+
   // Gains are for example purposes only - must be determined for your own robot!
   private final PIDController m_drivePIDController = new PIDController(1, 0, 0);
 
@@ -76,7 +81,8 @@ public SwerveModule(
     // resolution.
     m_driveEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
 
-    // Set the distance (in this case, angle) in radians per pulse for the turning encoder.
+    // Set the distance (in this case, angle) in radians per pulse for the turning
+    // encoder.
     // This is the the angle through an entire rotation (2 * pi) divided by the
     // encoder resolution.
     m_turningEncoder.setDistancePerPulse(2 * Math.PI / kEncoderResolution);
@@ -92,8 +98,9 @@ public SwerveModule(
    * @return The current state of the module.
    */
   public SwerveModuleState getState() {
-    return new SwerveModuleState(
-        m_driveEncoder.getRate(), new Rotation2d(m_turningEncoder.getDistance()));
+    m_moduleAngle.mut_fromRadians(m_turningEncoder.getDistance());
+    m_moduleState.setState(m_driveEncoder.getRate(), m_moduleAngle);
+    return m_moduleState;
   }
 
   /**
@@ -109,30 +116,33 @@ public SwerveModulePosition getPosition() {
   /**
    * Sets the desired state for the module.
    *
-   * @param desiredState Desired state with speed and angle.
+   * @param state Desired state with speed and angle.
    */
-  public void setDesiredState(SwerveModuleState desiredState) {
-    var encoderRotation = new Rotation2d(m_turningEncoder.getDistance());
+  public void setDesiredState(SwerveModuleState state) {
+    m_encoderRotation.mut_fromRadians(m_turningEncoder.getDistance());
 
     // Optimize the reference state to avoid spinning further than 90 degrees
-    SwerveModuleState state = SwerveModuleState.optimize(desiredState, encoderRotation);
+    state.optimize(m_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
+    // 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(m_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(), state.getSpeedMetersPerSecond());
 
     final double driveFeedforward =
-        m_driveFeedforward.calculate(MetersPerSecond.of(state.speedMetersPerSecond)).in(Volts);
+        m_driveFeedforward.calculate(MetersPerSecond.of(state.getSpeedMetersPerSecond())).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(), state.getAngle().getRadians());
 
     final double turnFeedforward =
         m_turnFeedforward

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

@@ -6,6 +6,7 @@
 
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.geometry.MutRotation2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
@@ -21,6 +22,10 @@ public class SwerveModule {
   private final Encoder m_driveEncoder;
   private final Encoder m_turningEncoder;
 
+  private final MutRotation2d m_moduleAngle = new MutRotation2d();
+  private final SwerveModuleState m_moduleState = new SwerveModuleState();
+  private final MutRotation2d m_encoderRotation = new MutRotation2d();
+
   private final PIDController m_drivePIDController =
       new PIDController(ModuleConstants.kPModuleDriveController, 0, 0);
 
@@ -85,8 +90,9 @@ public SwerveModule(
    * @return The current state of the module.
    */
   public SwerveModuleState getState() {
-    return new SwerveModuleState(
-        m_driveEncoder.getRate(), new Rotation2d(m_turningEncoder.getDistance()));
+    m_moduleAngle.mut_fromRadians(m_turningEncoder.getDistance());
+    m_moduleState.setState(m_driveEncoder.getRate(), m_moduleAngle);
+    return m_moduleState;
   }
 
   /**
@@ -102,26 +108,28 @@ public SwerveModulePosition getPosition() {
   /**
    * Sets the desired state for the module.
    *
-   * @param desiredState Desired state with speed and angle.
+   * @param state Desired state with speed and angle.
    */
-  public void setDesiredState(SwerveModuleState desiredState) {
-    var encoderRotation = new Rotation2d(m_turningEncoder.getDistance());
+  public void setDesiredState(SwerveModuleState state) {
+    m_encoderRotation.mut_fromRadians(m_turningEncoder.getDistance());
 
     // Optimize the reference state to avoid spinning further than 90 degrees
-    SwerveModuleState state = SwerveModuleState.optimize(desiredState, encoderRotation);
+    state.optimize(m_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();
+    state.setSpeed(
+        state.getSpeedMetersPerSecond() * state.getAngle().minus(m_encoderRotation).getCos());
 
     // 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(), state.getSpeedMetersPerSecond());
 
     // 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(), state.getAngle().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

@@ -11,6 +11,7 @@
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.geometry.MutRotation2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
@@ -32,6 +33,10 @@ public class SwerveModule {
   private final Encoder m_driveEncoder;
   private final Encoder m_turningEncoder;
 
+  private final MutRotation2d m_moduleAngle = new MutRotation2d();
+  private final SwerveModuleState m_moduleState = new SwerveModuleState();
+  private final MutRotation2d m_encoderRotation = new MutRotation2d();
+
   // Gains are for example purposes only - must be determined for your own robot!
   private final PIDController m_drivePIDController = new PIDController(1, 0, 0);
 
@@ -92,8 +97,9 @@ public SwerveModule(
    * @return The current state of the module.
    */
   public SwerveModuleState getState() {
-    return new SwerveModuleState(
-        m_driveEncoder.getRate(), new Rotation2d(m_turningEncoder.getDistance()));
+    m_moduleAngle.mut_fromRadians(m_turningEncoder.getDistance());
+    m_moduleState.setState(m_driveEncoder.getRate(), m_moduleAngle);
+    return m_moduleState;
   }
 
   /**
@@ -109,29 +115,31 @@ public SwerveModulePosition getPosition() {
   /**
    * Sets the desired state for the module.
    *
-   * @param desiredState Desired state with speed and angle.
+   * @param state Desired state with speed and angle.
    */
-  public void setDesiredState(SwerveModuleState desiredState) {
-    var encoderRotation = new Rotation2d(m_turningEncoder.getDistance());
+  public void setDesiredState(SwerveModuleState state) {
+    m_encoderRotation.mut_fromRadians(m_turningEncoder.getDistance());
 
     // Optimize the reference state to avoid spinning further than 90 degrees
-    SwerveModuleState state = SwerveModuleState.optimize(desiredState, encoderRotation);
+    state.optimize(m_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();
+    state.setSpeed(
+        state.getSpeedMetersPerSecond() * state.getAngle().minus(m_encoderRotation).getCos());
 
     // 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(), state.getSpeedMetersPerSecond());
 
     final double driveFeedforward =
-        m_driveFeedforward.calculate(MetersPerSecond.of(state.speedMetersPerSecond)).in(Volts);
+        m_driveFeedforward.calculate(MetersPerSecond.of(state.getSpeedMetersPerSecond())).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(), state.getAngle().getRadians());
 
     final double turnFeedforward =
         m_turnFeedforward