Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Adjust applyDeadband logic (#7816)

Browse Source 
Reduces nesting by returning when the value is within the deadband.

Adjusts the algorithm to handle large values of maxMagnitude naturally (instead of needing a separate check).

Reformats the math comments.
This commit is contained in:
Joseph Eng
2025-04-23 09:41:03 -05:00
committed by GitHub
parent 1c35a3a5ff
commit d98ad815b1
2 changed files with 91 additions and 86 deletions
Download Patch File Download Diff File Expand all files Collapse all files

87
wpimath/src/main/java/edu/wpi/first/math/MathUtil.java
View File

@@ -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);
}
}