Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +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);
}
/**

23
wpimath/src/main/native/cpp/MathUtil.cpp
View File

@@ -1,23 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/MathUtil.h"
#include <cmath>
namespace frc {
double ApplyDeadband(double value, double deadband) {
if (std::abs(value) > deadband) {
if (value > 0.0) {
return (value - deadband) / (1.0 - deadband);
} else {
return (value + deadband) / (1.0 - deadband);
}
} else {
return 0.0;
}
}
} // namespace frc

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

@@ -8,18 +8,79 @@
#include <wpi/numbers>
#include "units/angle.h"
#include "units/base.h"
#include "units/math.h"
namespace frc {
/**
* 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.
* 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 value Value to clip.
* @param deadband Range around zero.
* @param maxMagnitude The maximum magnitude of the input (defaults to 1). Can
* be infinite.
* @return The value after the deadband is applied.
*/
WPILIB_DLLEXPORT
double ApplyDeadband(double value, double deadband);
template <typename T,
typename = std::enable_if_t<std::disjunction_v<
std::is_floating_point<T>, units::traits::is_unit_t<T>>>>
T ApplyDeadband(T value, T deadband, T maxMagnitude = T{1.0}) {
T magnitude;
if constexpr (std::is_floating_point_v<T>) {
magnitude = std::abs(value);
} else {
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 {
return T{0.0};
}
}
/**
* Returns modulus of input.

23
wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java
View File

@@ -10,7 +10,7 @@ import org.junit.jupiter.api.Test;
class MathUtilTest {
@Test
void testApplyDeadband() {
void testApplyDeadbandUnityScale() {
// < 0
assertEquals(-1.0, MathUtil.applyDeadband(-1.0, 0.02));
assertEquals((-0.03 + 0.02) / (1.0 - 0.02), MathUtil.applyDeadband(-0.03, 0.02));
@@ -27,6 +27,27 @@ class MathUtilTest {
assertEquals(1.0, MathUtil.applyDeadband(1.0, 0.02));
}
@Test
void testApplyDeadbandArbitraryScale() {
// < 0
assertEquals(-2.5, MathUtil.applyDeadband(-2.5, 0.02, 2.5));
assertEquals(0.0, MathUtil.applyDeadband(-0.02, 0.02, 2.5));
assertEquals(0.0, MathUtil.applyDeadband(-0.01, 0.02, 2.5));
// == 0
assertEquals(0.0, MathUtil.applyDeadband(0.0, 0.02, 2.5));
// > 0
assertEquals(0.0, MathUtil.applyDeadband(0.01, 0.02, 2.5));
assertEquals(0.0, MathUtil.applyDeadband(0.02, 0.02, 2.5));
assertEquals(2.5, MathUtil.applyDeadband(2.5, 0.02, 2.5));
}
@Test
void testApplyDeadbandLargeMaxMagnitude() {
assertEquals(80.0, MathUtil.applyDeadband(100.0, 20, Double.POSITIVE_INFINITY));
}
@Test
void testInputModulus() {
// These tests check error wrapping. That is, the result of wrapping the

31
wpimath/src/test/native/cpp/MathUtilTest.cpp
View File

@@ -2,6 +2,8 @@
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include <limits>
#include "frc/MathUtil.h"
#include "gtest/gtest.h"
#include "units/angle.h"
@@ -10,7 +12,7 @@
#define EXPECT_UNITS_NEAR(a, b, c) EXPECT_NEAR((a).value(), (b).value(), c)
TEST(MathUtilTest, ApplyDeadband) {
TEST(MathUtilTest, ApplyDeadbandUnityScale) {
// < 0
EXPECT_DOUBLE_EQ(-1.0, frc::ApplyDeadband(-1.0, 0.02));
EXPECT_DOUBLE_EQ((-0.03 + 0.02) / (1.0 - 0.02),
@@ -29,6 +31,33 @@ TEST(MathUtilTest, ApplyDeadband) {
EXPECT_DOUBLE_EQ(1.0, frc::ApplyDeadband(1.0, 0.02));
}
TEST(MathUtilTest, ApplyDeadbandArbitraryScale) {
// < 0
EXPECT_DOUBLE_EQ(-2.5, frc::ApplyDeadband(-2.5, 0.02, 2.5));
EXPECT_DOUBLE_EQ(0.0, frc::ApplyDeadband(-0.02, 0.02, 2.5));
EXPECT_DOUBLE_EQ(0.0, frc::ApplyDeadband(-0.01, 0.02, 2.5));
// == 0
EXPECT_DOUBLE_EQ(0.0, frc::ApplyDeadband(0.0, 0.02, 2.5));
// > 0
EXPECT_DOUBLE_EQ(0.0, frc::ApplyDeadband(0.01, 0.02, 2.5));
EXPECT_DOUBLE_EQ(0.0, frc::ApplyDeadband(0.02, 0.02, 2.5));
EXPECT_DOUBLE_EQ(2.5, frc::ApplyDeadband(2.5, 0.02, 2.5));
}
TEST(MathUtilTest, ApplyDeadbandUnits) {
// < 0
EXPECT_DOUBLE_EQ(
-20, frc::ApplyDeadband<units::radian_t>(-20_rad, 1_rad, 20_rad).value());
}
TEST(MathUtilTest, ApplyDeadbandLargeMaxMagnitude) {
EXPECT_DOUBLE_EQ(
80.0,
frc::ApplyDeadband(100.0, 20.0, std::numeric_limits<double>::infinity()));
}
TEST(MathUtilTest, InputModulus) {
// These tests check error wrapping. That is, the result of wrapping the
// result of an angle reference minus the measurement.