Address review

Author: Gold856

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

@@ -54,7 +54,6 @@ void SwerveModule::SetDesiredState(frc::SwerveModuleState& referenceState) {
   // then 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.
-  // Optimize the reference state to avoid spinning further than 90 degrees
   auto state =
       referenceState.Optimize(encoderRotation).CosineScale(encoderRotation);
 

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

@@ -63,7 +63,9 @@ public void drive(
     }
     chassisSpeeds = chassisSpeeds.discretize(period);
 
-    var states = SwerveDriveKinematics.desaturateWheelSpeeds(m_kinematics.toWheelSpeeds(chassisSpeeds), kMaxSpeed);
+    var states =
+        SwerveDriveKinematics.desaturateWheelSpeeds(
+            m_kinematics.toWheelSpeeds(chassisSpeeds), kMaxSpeed);
 
     m_frontLeft.setDesiredState(states[0]);
     m_frontRight.setDesiredState(states[1]);

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

@@ -74,7 +74,9 @@ public void drive(
 
     chassisSpeeds = chassisSpeeds.discretize(period);
 
-    var states = SwerveDriveKinematics.desaturateWheelSpeeds(m_kinematics.toWheelSpeeds(chassisSpeeds), kMaxSpeed);
+    var states =
+        SwerveDriveKinematics.desaturateWheelSpeeds(
+            m_kinematics.toWheelSpeeds(chassisSpeeds), kMaxSpeed);
 
     m_frontLeft.setDesiredState(states[0]);
     m_frontRight.setDesiredState(states[1]);

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

@@ -271,16 +271,21 @@ public static SwerveModuleState[] desaturateWheelSpeeds(
     for (SwerveModuleState moduleState : moduleStates) {
       realMaxSpeed = Math.max(realMaxSpeed, Math.abs(moduleState.speed));
     }
+    var states = new SwerveModuleState[moduleStates.length];
     if (realMaxSpeed > attainableMaxSpeed) {
-      var states = new SwerveModuleState[moduleStates.length];
       for (int i = 0; i < states.length; i++) {
         states[i] =
             new SwerveModuleState(
                 moduleStates[i].speed / realMaxSpeed * attainableMaxSpeed, moduleStates[i].angle);
       }
-      return states;
+    } else {
+      // Copy in the event someone wants to mutate the desaturated states but also wants the
+      // original states
+      for (int i = 0; i < states.length; i++) {
+        states[i] = new SwerveModuleState(moduleStates[i].speed, moduleStates[i].angle);
+      }
     }
-    return moduleStates;
+    return states;
   }
 
   /**
@@ -338,18 +343,23 @@ public static SwerveModuleState[] desaturateWheelSpeeds(
       realMaxSpeed = Math.max(realMaxSpeed, Math.abs(moduleState.speed));
     }
 
+    var states = new SwerveModuleState[moduleStates.length];
     if (attainableMaxTranslationalSpeed == 0
         || attainableMaxRotationalVelocity == 0
         || realMaxSpeed == 0) {
-      return moduleStates;
+      // Copy in the event someone wants to mutate the desaturated states but also wants the
+      // original states
+      for (int i = 0; i < states.length; i++) {
+        states[i] = new SwerveModuleState(moduleStates[i].speed, moduleStates[i].angle);
+      }
+      return states;
     }
     double translationalK =
         Math.hypot(desiredChassisSpeed.vx, desiredChassisSpeed.vy)
             / attainableMaxTranslationalSpeed;
     double rotationalK = Math.abs(desiredChassisSpeed.omega) / attainableMaxRotationalVelocity;
     double k = Math.max(translationalK, rotationalK);
     double scale = Math.min(k * attainableMaxModuleSpeed / realMaxSpeed, 1);
-    var states = new SwerveModuleState[moduleStates.length];
     for (int i = 0; i < states.length; i++) {
       states[i] = new SwerveModuleState(moduleStates[i].speed * scale, moduleStates[i].angle);
     }

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

@@ -144,7 +144,7 @@ class SwerveDriveKinematics
    * @return An array containing the module states. Use caution because these
    * module states are not normalized. Sometimes, a user input may cause one of
    * the module speeds to go above the attainable max velocity. Use the
-   * DesaturateWheelSpeeds(wpi::array<SwerveModuleState, NumModules>&,
+   * DesaturateWheelSpeeds(wpi::array<SwerveModuleState, NumModules>,
    * units::meters_per_second_t) function to rectify this issue. In addition,
    * you can leverage the power of C++17 to directly assign the module states to
    * variables:
@@ -353,15 +353,20 @@ class SwerveDriveKinematics
                          })
             ->speed);
 
+    wpi::array<SwerveModuleState, NumModules> states(wpi::empty_array);
     if (realMaxSpeed > attainableMaxSpeed) {
-      wpi::array<SwerveModuleState, NumModules> states(wpi::empty_array);
       for (size_t i = 0; i < NumModules; ++i) {
         states[i] = {moduleStates[i].speed / realMaxSpeed * attainableMaxSpeed,
                      moduleStates[i].angle};
       }
-      return states;
+    } else {
+      // Copy in the event someone wants to mutate the desaturated states but
+      // also wants the original states
+      for (size_t i = 0; i < NumModules; ++i) {
+        states[i] = moduleStates[i];
+      }
     }
-    return moduleStates;
+    return states;
   }
 
   /**
@@ -404,10 +409,16 @@ class SwerveDriveKinematics
                          })
             ->speed);
 
+    wpi::array<SwerveModuleState, NumModules> states(wpi::empty_array);
     if (attainableMaxRobotTranslationSpeed == 0_mps ||
         attainableMaxRobotRotationSpeed == 0_rad_per_s ||
         realMaxSpeed == 0_mps) {
-      return moduleStates;
+      // Copy in the event someone wants to mutate the desaturated states but
+      // also wants the original states
+      for (size_t i = 0; i < NumModules; ++i) {
+        states[i] = moduleStates[i];
+      }
+      return states;
     }
 
     auto translationalK =
@@ -421,7 +432,6 @@ class SwerveDriveKinematics
 
     auto scale = units::math::min(k * attainableMaxModuleSpeed / realMaxSpeed,
                                   units::scalar_t{1});
-    wpi::array<SwerveModuleState, NumModules> states(wpi::empty_array);
     for (size_t i = 0; i < NumModules; ++i) {
       states[i] = {moduleStates[i].speed * scale, moduleStates[i].angle};
     }

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

@@ -82,8 +82,8 @@ void testCosineScale() {
     var optimizedD = refD.cosineScale(angleD);
 
     assertAll(
-        () -> assertEquals(0.0, refD.speed, kEpsilon),
-        () -> assertEquals(45.0, refD.angle.getDegrees(), kEpsilon));
+        () -> assertEquals(0.0, optimizedD.speed, kEpsilon),
+        () -> assertEquals(45.0, optimizedD.angle.getDegrees(), kEpsilon));
 
     var angleE = Rotation2d.fromDegrees(-135.0);
     var refE = new SwerveModuleState(2.0, Rotation2d.fromDegrees(45.0));