[wpimath] MecanumDriveWheelSpeeds: Fix desaturate() (#6040)

wpilibsuite · Dec 15, 2023 · f87c64a · f87c64a
1 parent 8798700
commit f87c64a
Show file tree

Hide file tree

Showing 4 changed files with 31 additions and 10 deletions.
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java
@@ -11,7 +11,6 @@
 import edu.wpi.first.units.Distance;
 import edu.wpi.first.units.Measure;
 import edu.wpi.first.units.Velocity;
-import java.util.stream.DoubleStream;
 
 public class MecanumDriveWheelSpeeds {
   /** Speed of the front left wheel. */
@@ -83,13 +82,9 @@ public MecanumDriveWheelSpeeds(
    */
   public void desaturate(double attainableMaxSpeedMetersPerSecond) {
     double realMaxSpeed =
-        DoubleStream.of(
-                frontLeftMetersPerSecond,
-                frontRightMetersPerSecond,
-                rearLeftMetersPerSecond,
-                rearRightMetersPerSecond)
-            .max()
-            .getAsDouble();
+        Math.max(Math.abs(frontLeftMetersPerSecond), Math.abs(frontRightMetersPerSecond));
+    realMaxSpeed = Math.max(realMaxSpeed, Math.abs(rearLeftMetersPerSecond));
+    realMaxSpeed = Math.max(realMaxSpeed, Math.abs(rearRightMetersPerSecond));
 
     if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) {
       frontLeftMetersPerSecond =

diff --git a/wpimath/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp b/wpimath/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp
@@ -16,10 +16,10 @@ void MecanumDriveWheelSpeeds::Desaturate(
     units::meters_per_second_t attainableMaxSpeed) {
   std::array<units::meters_per_second_t, 4> wheelSpeeds{frontLeft, frontRight,
                                                         rearLeft, rearRight};
-  units::meters_per_second_t realMaxSpeed = *std::max_element(
+  units::meters_per_second_t realMaxSpeed = units::math::abs(*std::max_element(
       wheelSpeeds.begin(), wheelSpeeds.end(), [](const auto& a, const auto& b) {
         return units::math::abs(a) < units::math::abs(b);
-      });
+      }));
 
   if (realMaxSpeed > attainableMaxSpeed) {
     for (int i = 0; i < 4; ++i) {

diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveKinematicsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveKinematicsTest.java
@@ -238,4 +238,18 @@ void testDesaturate() {
         () -> assertEquals(4.0 * factor, wheelSpeeds.rearLeftMetersPerSecond, kEpsilon),
         () -> assertEquals(7.0 * factor, wheelSpeeds.rearRightMetersPerSecond, kEpsilon));
   }
+
+  @Test
+  void testDesaturateNegativeSpeeds() {
+    var wheelSpeeds = new MecanumDriveWheelSpeeds(-5, 6, 4, -7);
+    wheelSpeeds.desaturate(5.5);
+
+    final double kFactor = 5.5 / 7.0;
+
+    assertAll(
+        () -> assertEquals(-5.0 * kFactor, wheelSpeeds.frontLeftMetersPerSecond, kEpsilon),
+        () -> assertEquals(6.0 * kFactor, wheelSpeeds.frontRightMetersPerSecond, kEpsilon),
+        () -> assertEquals(4.0 * kFactor, wheelSpeeds.rearLeftMetersPerSecond, kEpsilon),
+        () -> assertEquals(-7.0 * kFactor, wheelSpeeds.rearRightMetersPerSecond, kEpsilon));
+  }
 }
diff --git a/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp
@@ -215,3 +215,15 @@ TEST_F(MecanumDriveKinematicsTest, Desaturate) {
   EXPECT_NEAR(wheelSpeeds.rearLeft.value(), 4.0 * kFactor, 1E-9);
   EXPECT_NEAR(wheelSpeeds.rearRight.value(), 7.0 * kFactor, 1E-9);
 }
+
+TEST_F(MecanumDriveKinematicsTest, DesaturateNegativeSpeeds) {
+  MecanumDriveWheelSpeeds wheelSpeeds{-5_mps, 6_mps, 4_mps, -7_mps};
+  wheelSpeeds.Desaturate(5.5_mps);
+
+  constexpr double kFactor = 5.5 / 7.0;
+
+  EXPECT_NEAR(wheelSpeeds.frontLeft.value(), -5.0 * kFactor, 1E-9);
+  EXPECT_NEAR(wheelSpeeds.frontRight.value(), 6.0 * kFactor, 1E-9);
+  EXPECT_NEAR(wheelSpeeds.rearLeft.value(), 4.0 * kFactor, 1E-9);
+  EXPECT_NEAR(wheelSpeeds.rearRight.value(), -7.0 * kFactor, 1E-9);
+}