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 1836005cee..1e374ec583 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java +++ b/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java @@ -47,50 +47,51 @@ public final class MathUtil { * @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); - } + if (Math.abs(value) < deadband) { + return 0; + } + if (value > 0.0) { + // Map deadband to 0 and map max to max with a linear relationship. + // + // 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) + // + // To handle high values of max, rewrite so that max only appears on the denominator. + // + // y = ((max - deadband) + deadband)/(max - deadband) (x - deadband) + // y = (1 + deadband/(max - deadband)) (x - deadband) + return (1 + deadband / (maxMagnitude - deadband)) * (value - deadband); } else { - return 0.0; + // Map -deadband to 0 and map -max to -max with a linear relationship. + // + // 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) + // + // To handle high values of max, rewrite so that max only appears on the denominator. + // + // y = ((max - deadband) + deadband)/(max - deadband) (x + deadband) + // y = (1 + deadband/(max - deadband)) (x + deadband) + return (1 + deadband / (maxMagnitude - deadband)) * (value + deadband); } } diff --git a/wpimath/src/main/native/include/frc/MathUtil.h b/wpimath/src/main/native/include/frc/MathUtil.h index e6ec32908d..060be0da91 100644 --- a/wpimath/src/main/native/include/frc/MathUtil.h +++ b/wpimath/src/main/native/include/frc/MathUtil.h @@ -43,51 +43,55 @@ constexpr T ApplyDeadband(T value, T deadband, T maxMagnitude = T{1.0}) { 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 { + if (magnitude < deadband) { return T{0.0}; } + + if (value > T{0.0}) { + // Map deadband to 0 and map max to max with a linear relationship. + // + // 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) + // + // To handle high values of max, rewrite so that max only appears on the + // denominator. + // + // y = ((max - deadband) + deadband)/(max - deadband) (x - deadband) + // y = (1 + deadband/(max - deadband)) (x - deadband) + return (1.0 + deadband / (maxMagnitude - deadband)) * (value - deadband); + } else { + // Map -deadband to 0 and map -max to -max with a linear relationship. + // + // 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) + // + // To handle high values of max, rewrite so that max only appears on the + // denominator. + // + // y = ((max - deadband) + deadband)/(max - deadband) (x + deadband) + // y = (1 + deadband/(max - deadband)) (x + deadband) + return (1.0 + deadband / (maxMagnitude - deadband)) * (value + deadband); + } } /**