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] ApplyDeadband: add a scale param (#3865)

Browse Source 
Also templates it in C++ so it can work with both doubles and units.
This commit is contained in:
Oblarg
2022-04-30 23:29:48 -04:00
committed by GitHub
parent 03230fc842
commit 09cf6eeecb
5 changed files with 176 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

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