diff --git a/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java b/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java index 25b9f9258a..a299ae359e 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java +++ b/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java @@ -33,6 +33,63 @@ public final class MathUtil { return Math.max(low, Math.min(value, high)); } + /** + * Returns 0.0 if the given value is within the specified range around zero. The remaining range + * between the deadband and the maximum magnitude is scaled from 0.0 to the maximum magnitude. + * + * @param value Value to clip. + * @param deadband Range around zero. + * @param maxMagnitude The maximum magnitude of the input. Can be infinite. + * @return The value after the deadband is applied. + */ + public static double applyDeadband(double value, double deadband, double maxMagnitude) { + if (Math.abs(value) > deadband) { + if (maxMagnitude / deadband > 1.0e12) { + // If max magnitude is sufficiently large, the implementation encounters + // roundoff error. Implementing the limiting behavior directly avoids + // the problem. + return value > 0.0 ? value - deadband : value + deadband; + } + if (value > 0.0) { + // Map deadband to 0 and map max to max. + // + // y - y₁ = m(x - x₁) + // y - y₁ = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + // y = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + y₁ + // + // (x₁, y₁) = (deadband, 0) and (x₂, y₂) = (max, max). + // x₁ = deadband + // y₁ = 0 + // x₂ = max + // y₂ = max + // + // y = (max - 0)/(max - deadband) (x - deadband) + 0 + // y = max/(max - deadband) (x - deadband) + // y = max (x - deadband)/(max - deadband) + return maxMagnitude * (value - deadband) / (maxMagnitude - deadband); + } else { + // Map -deadband to 0 and map -max to -max. + // + // y - y₁ = m(x - x₁) + // y - y₁ = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + // y = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + y₁ + // + // (x₁, y₁) = (-deadband, 0) and (x₂, y₂) = (-max, -max). + // x₁ = -deadband + // y₁ = 0 + // x₂ = -max + // y₂ = -max + // + // y = (-max - 0)/(-max + deadband) (x + deadband) + 0 + // y = max/(max - deadband) (x + deadband) + // y = max (x + deadband)/(max - deadband) + return maxMagnitude * (value + deadband) / (maxMagnitude - deadband); + } + } else { + return 0.0; + } + } + /** * Returns 0.0 if the given value is within the specified range around zero. The remaining range * between the deadband and 1.0 is scaled from 0.0 to 1.0. @@ -42,15 +99,7 @@ public final class MathUtil { * @return The value after the deadband is applied. */ public static double applyDeadband(double value, double deadband) { - if (Math.abs(value) > deadband) { - if (value > 0.0) { - return (value - deadband) / (1.0 - deadband); - } else { - return (value + deadband) / (1.0 - deadband); - } - } else { - return 0.0; - } + return applyDeadband(value, deadband, 1); } /** diff --git a/wpimath/src/main/native/cpp/MathUtil.cpp b/wpimath/src/main/native/cpp/MathUtil.cpp deleted file mode 100644 index 19cd214ead..0000000000 --- a/wpimath/src/main/native/cpp/MathUtil.cpp +++ /dev/null @@ -1,23 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -#include "frc/MathUtil.h" - -#include - -namespace frc { - -double ApplyDeadband(double value, double deadband) { - if (std::abs(value) > deadband) { - if (value > 0.0) { - return (value - deadband) / (1.0 - deadband); - } else { - return (value + deadband) / (1.0 - deadband); - } - } else { - return 0.0; - } -} - -} // namespace frc diff --git a/wpimath/src/main/native/include/frc/MathUtil.h b/wpimath/src/main/native/include/frc/MathUtil.h index 54a77afb99..377117170f 100644 --- a/wpimath/src/main/native/include/frc/MathUtil.h +++ b/wpimath/src/main/native/include/frc/MathUtil.h @@ -8,18 +8,79 @@ #include #include "units/angle.h" +#include "units/base.h" +#include "units/math.h" namespace frc { /** - * Returns 0.0 if the given value is within the specified range around zero. - * The remaining range between the deadband and 1.0 is scaled from 0.0 to 1.0. + * Returns 0.0 if the given value is within the specified range around zero. The + * remaining range between the deadband and the maximum magnitude is scaled from + * 0.0 to the maximum magnitude. * - * @param value Value to clip. + * @param value Value to clip. * @param deadband Range around zero. + * @param maxMagnitude The maximum magnitude of the input (defaults to 1). Can + * be infinite. + * @return The value after the deadband is applied. */ -WPILIB_DLLEXPORT -double ApplyDeadband(double value, double deadband); +template , units::traits::is_unit_t>>> +T ApplyDeadband(T value, T deadband, T maxMagnitude = T{1.0}) { + T magnitude; + if constexpr (std::is_floating_point_v) { + magnitude = std::abs(value); + } else { + magnitude = units::math::abs(value); + } + + if (magnitude > deadband) { + if (maxMagnitude / deadband > 1.0E12) { + // If max magnitude is sufficiently large, the implementation encounters + // roundoff error. Implementing the limiting behavior directly avoids + // the problem. + return value > T{0.0} ? value - deadband : value + deadband; + } + if (value > T{0.0}) { + // Map deadband to 0 and map max to max. + // + // y - y₁ = m(x - x₁) + // y - y₁ = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + // y = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + y₁ + // + // (x₁, y₁) = (deadband, 0) and (x₂, y₂) = (max, max). + // x₁ = deadband + // y₁ = 0 + // x₂ = max + // y₂ = max + // + // y = (max - 0)/(max - deadband) (x - deadband) + 0 + // y = max/(max - deadband) (x - deadband) + // y = max (x - deadband)/(max - deadband) + return maxMagnitude * (value - deadband) / (maxMagnitude - deadband); + } else { + // Map -deadband to 0 and map -max to -max. + // + // y - y₁ = m(x - x₁) + // y - y₁ = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + // y = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + y₁ + // + // (x₁, y₁) = (-deadband, 0) and (x₂, y₂) = (-max, -max). + // x₁ = -deadband + // y₁ = 0 + // x₂ = -max + // y₂ = -max + // + // y = (-max - 0)/(-max + deadband) (x + deadband) + 0 + // y = max/(max - deadband) (x + deadband) + // y = max (x + deadband)/(max - deadband) + return maxMagnitude * (value + deadband) / (maxMagnitude - deadband); + } + } else { + return T{0.0}; + } +} /** * Returns modulus of input. diff --git a/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java b/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java index bb116cee7a..a3de9ccd56 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java @@ -10,7 +10,7 @@ import org.junit.jupiter.api.Test; class MathUtilTest { @Test - void testApplyDeadband() { + void testApplyDeadbandUnityScale() { // < 0 assertEquals(-1.0, MathUtil.applyDeadband(-1.0, 0.02)); assertEquals((-0.03 + 0.02) / (1.0 - 0.02), MathUtil.applyDeadband(-0.03, 0.02)); @@ -27,6 +27,27 @@ class MathUtilTest { assertEquals(1.0, MathUtil.applyDeadband(1.0, 0.02)); } + @Test + void testApplyDeadbandArbitraryScale() { + // < 0 + assertEquals(-2.5, MathUtil.applyDeadband(-2.5, 0.02, 2.5)); + assertEquals(0.0, MathUtil.applyDeadband(-0.02, 0.02, 2.5)); + assertEquals(0.0, MathUtil.applyDeadband(-0.01, 0.02, 2.5)); + + // == 0 + assertEquals(0.0, MathUtil.applyDeadband(0.0, 0.02, 2.5)); + + // > 0 + assertEquals(0.0, MathUtil.applyDeadband(0.01, 0.02, 2.5)); + assertEquals(0.0, MathUtil.applyDeadband(0.02, 0.02, 2.5)); + assertEquals(2.5, MathUtil.applyDeadband(2.5, 0.02, 2.5)); + } + + @Test + void testApplyDeadbandLargeMaxMagnitude() { + assertEquals(80.0, MathUtil.applyDeadband(100.0, 20, Double.POSITIVE_INFINITY)); + } + @Test void testInputModulus() { // These tests check error wrapping. That is, the result of wrapping the diff --git a/wpimath/src/test/native/cpp/MathUtilTest.cpp b/wpimath/src/test/native/cpp/MathUtilTest.cpp index 6b5af2baf5..8e73ee1bbf 100644 --- a/wpimath/src/test/native/cpp/MathUtilTest.cpp +++ b/wpimath/src/test/native/cpp/MathUtilTest.cpp @@ -2,6 +2,8 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. +#include + #include "frc/MathUtil.h" #include "gtest/gtest.h" #include "units/angle.h" @@ -10,7 +12,7 @@ #define EXPECT_UNITS_NEAR(a, b, c) EXPECT_NEAR((a).value(), (b).value(), c) -TEST(MathUtilTest, ApplyDeadband) { +TEST(MathUtilTest, ApplyDeadbandUnityScale) { // < 0 EXPECT_DOUBLE_EQ(-1.0, frc::ApplyDeadband(-1.0, 0.02)); EXPECT_DOUBLE_EQ((-0.03 + 0.02) / (1.0 - 0.02), @@ -29,6 +31,33 @@ TEST(MathUtilTest, ApplyDeadband) { EXPECT_DOUBLE_EQ(1.0, frc::ApplyDeadband(1.0, 0.02)); } +TEST(MathUtilTest, ApplyDeadbandArbitraryScale) { + // < 0 + EXPECT_DOUBLE_EQ(-2.5, frc::ApplyDeadband(-2.5, 0.02, 2.5)); + EXPECT_DOUBLE_EQ(0.0, frc::ApplyDeadband(-0.02, 0.02, 2.5)); + EXPECT_DOUBLE_EQ(0.0, frc::ApplyDeadband(-0.01, 0.02, 2.5)); + + // == 0 + EXPECT_DOUBLE_EQ(0.0, frc::ApplyDeadband(0.0, 0.02, 2.5)); + + // > 0 + EXPECT_DOUBLE_EQ(0.0, frc::ApplyDeadband(0.01, 0.02, 2.5)); + EXPECT_DOUBLE_EQ(0.0, frc::ApplyDeadband(0.02, 0.02, 2.5)); + EXPECT_DOUBLE_EQ(2.5, frc::ApplyDeadband(2.5, 0.02, 2.5)); +} + +TEST(MathUtilTest, ApplyDeadbandUnits) { + // < 0 + EXPECT_DOUBLE_EQ( + -20, frc::ApplyDeadband(-20_rad, 1_rad, 20_rad).value()); +} + +TEST(MathUtilTest, ApplyDeadbandLargeMaxMagnitude) { + EXPECT_DOUBLE_EQ( + 80.0, + frc::ApplyDeadband(100.0, 20.0, std::numeric_limits::infinity())); +} + TEST(MathUtilTest, InputModulus) { // These tests check error wrapping. That is, the result of wrapping the // result of an angle reference minus the measurement.