Parity with other wheel speed classes

Author: Gold856

wpimath/src/main/java/org/wpilib/math/kinematics/DifferentialDriveWheelVelocities.java

@@ -6,6 +6,7 @@
 
 import static org.wpilib.units.Units.MetersPerSecond;
 
+import org.wpilib.annotation.NoDiscard;
 import org.wpilib.math.interpolation.Interpolatable;
 import org.wpilib.math.kinematics.proto.DifferentialDriveWheelVelocitiesProto;
 import org.wpilib.math.kinematics.struct.DifferentialDriveWheelVelocitiesStruct;
@@ -14,6 +15,7 @@
 import org.wpilib.util.struct.StructSerializable;
 
 /** Represents the wheel velocities for a differential drive drivetrain. */
+@NoDiscard
 public class DifferentialDriveWheelVelocities
     implements Interpolatable<DifferentialDriveWheelVelocities>,
         ProtobufSerializable,
@@ -66,14 +68,17 @@ public DifferentialDriveWheelVelocities(LinearVelocity left, LinearVelocity righ
    *
    * @param attainableMaxVelocity The absolute max velocity in meters per second that a wheel can
    *     reach.
+   * @return The desaturated DifferentialDriveWheelVelocities.
    */
-  public void desaturate(double attainableMaxVelocity) {
+  public DifferentialDriveWheelVelocities desaturate(double attainableMaxVelocity) {
     double realMaxVelocity = Math.max(Math.abs(left), Math.abs(right));
 
     if (realMaxVelocity > attainableMaxVelocity) {
-      left = left / realMaxVelocity * attainableMaxVelocity;
-      right = right / realMaxVelocity * attainableMaxVelocity;
+      return new DifferentialDriveWheelVelocities(
+          left / realMaxVelocity * attainableMaxVelocity,
+          right / realMaxVelocity * attainableMaxVelocity);
     }
+    return new DifferentialDriveWheelVelocities(left, right);
   }
 
   /**
@@ -86,9 +91,10 @@ public void desaturate(double attainableMaxVelocity) {
    *
    * @param attainableMaxVelocity The absolute max velocity in meters per second that a wheel can
    *     reach.
+   * @return The desaturated DifferentialDriveWheelVelocities.
    */
-  public void desaturate(LinearVelocity attainableMaxVelocity) {
-    desaturate(attainableMaxVelocity.in(MetersPerSecond));
+  public DifferentialDriveWheelVelocities desaturate(LinearVelocity attainableMaxVelocity) {
+    return desaturate(attainableMaxVelocity.in(MetersPerSecond));
   }
 
   /**

wpimath/src/main/java/org/wpilib/math/kinematics/MecanumDriveWheelVelocities.java

@@ -6,6 +6,7 @@
 
 import static org.wpilib.units.Units.MetersPerSecond;
 
+import org.wpilib.annotation.NoDiscard;
 import org.wpilib.math.interpolation.Interpolatable;
 import org.wpilib.math.kinematics.proto.MecanumDriveWheelVelocitiesProto;
 import org.wpilib.math.kinematics.struct.MecanumDriveWheelVelocitiesStruct;
@@ -14,6 +15,7 @@
 import org.wpilib.util.struct.StructSerializable;
 
 /** Represents the wheel velocities for a mecanum drive drivetrain. */
+@NoDiscard
 public class MecanumDriveWheelVelocities
     implements Interpolatable<MecanumDriveWheelVelocities>,
         ProtobufSerializable,
@@ -87,7 +89,7 @@ public MecanumDriveWheelVelocities(
    *
    * @param attainableMaxVelocity The absolute max velocity in meters per second that a wheel can
    *     reach.
-   * @return Desaturated MecanumDriveWheelVelocities.
+   * @return The desaturated MecanumDriveWheelVelocities.
    */
   public MecanumDriveWheelVelocities desaturate(double attainableMaxVelocity) {
     double realMaxVelocity = Math.max(Math.abs(frontLeft), Math.abs(frontRight));

wpimath/src/main/java/org/wpilib/math/trajectory/constraint/DifferentialDriveKinematicsConstraint.java

@@ -44,8 +44,8 @@ public double getMaxVelocity(Pose2d pose, double curvature, double velocity) {
     var chassisVelocities = new ChassisVelocities(velocity, 0, velocity * curvature);
 
     // Get the wheel velocities and normalize them to within the max velocity.
-    var wheelVelocities = m_kinematics.toWheelVelocities(chassisVelocities);
-    wheelVelocities.desaturate(m_maxVelocity);
+    var wheelVelocities =
+        m_kinematics.toWheelVelocities(chassisVelocities).desaturate(m_maxVelocity);
 
     // Return the new linear chassis velocity.
     return m_kinematics.toChassisVelocities(wheelVelocities).vx;

wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveWheelVelocities.hpp

@@ -36,16 +36,18 @@ struct WPILIB_DLLEXPORT DifferentialDriveWheelVelocities {
    *
    * @param attainableMaxVelocity The absolute max velocity that a wheel can
    *     reach.
+   * @return The desaturated DifferentialDriveWheelVelocities.
    */
-  constexpr void Desaturate(
+  constexpr DifferentialDriveWheelVelocities Desaturate(
       wpi::units::meters_per_second_t attainableMaxVelocity) {
     auto realMaxVelocity = wpi::units::math::max(wpi::units::math::abs(left),
                                                  wpi::units::math::abs(right));
 
     if (realMaxVelocity > attainableMaxVelocity) {
-      left = left / realMaxVelocity * attainableMaxVelocity;
-      right = right / realMaxVelocity * attainableMaxVelocity;
+      return {left / realMaxVelocity * attainableMaxVelocity,
+              right / realMaxVelocity * attainableMaxVelocity};
     }
+    return *this;
   }
 
   /**

wpimath/src/main/native/include/wpi/math/trajectory/constraint/DifferentialDriveKinematicsConstraint.hpp

@@ -30,8 +30,8 @@ class WPILIB_DLLEXPORT DifferentialDriveKinematicsConstraint
       const Pose2d& pose, wpi::units::curvature_t curvature,
       wpi::units::meters_per_second_t velocity) const override {
     auto wheelVelocities =
-        m_kinematics.ToWheelVelocities({velocity, 0_mps, velocity * curvature});
-    wheelVelocities.Desaturate(m_maxVelocity);
+        m_kinematics.ToWheelVelocities({velocity, 0_mps, velocity * curvature})
+            .Desaturate(m_maxVelocity);
 
     return m_kinematics.ToChassisVelocities(wheelVelocities).vx;
   }

wpimath/src/main/native/include/wpi/math/trajectory/constraint/SwerveDriveKinematicsConstraint.hpp

@@ -32,10 +32,10 @@ class SwerveDriveKinematicsConstraint : public TrajectoryConstraint {
     auto wheelVelocities = m_kinematics.ToSwerveModuleVelocities(
         {xVelocity, yVelocity, velocity * curvature});
 
-    auto normSpeeds = m_kinematics.ToChassisVelocities(
+    auto normVelocities = m_kinematics.ToChassisVelocities(
         m_kinematics.DesaturateWheelVelocities(wheelVelocities, m_maxVelocity));
 
-    return wpi::units::math::hypot(normSpeeds.vx, normSpeeds.vy);
+    return wpi::units::math::hypot(normVelocities.vx, normVelocities.vy);
   }
 
   MinMax MinMaxAcceleration(