Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-23 01:21: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

90
wpimath/src/main/native/include/frc/MathUtil.h
View File

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