Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[wpimath] Adjust applyDeadband logic #7816

Open
wants to merge 1 commit into
base: main
Choose a base branch
from
Open

[wpimath] Adjust applyDeadband logic #7816

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
87 changes: 44 additions & 43 deletions wpimath/src/main/java/edu/wpi/first/math/MathUtil.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,50 +44,51 @@ public static double clamp(double value, double low, double high) {
* @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);
}
}

Expand Down
90 changes: 47 additions & 43 deletions wpimath/src/main/native/include/frc/MathUtil.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,51 +37,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);
}
}

/**
Expand Down
Loading