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 index 64d712aa10..2297b3111a 100644 --- 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.math.kinematics.struct.MecanumDriveWheelSpeedsStruct; 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 class 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 index dd30263fbc..0f5de422e2 100644 --- 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 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 index 18d3ec7af6..28fbfd8e83 100644 --- 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 @@ class MecanumDriveKinematicsTest { () -> 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 index 04a3f1f2d8..3fe1471222 100644 --- 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); +}