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

[wpimath] Add isNear method to MathUtil (#5353)

Browse Source 
This method is used to check if the given value matches an expected value within a certain tolerance.

Co-authored-by: Tyler Veness <calcmogul@gmail.com>
Co-authored-by: Ryan Blue <ryanzblue@gmail.com>
This commit is contained in:
Gold856
2023-06-20 02:00:07 -04:00
committed by GitHub
parent 2ce248f66c
commit f5b0d1484b
4 changed files with 189 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -145,4 +145,47 @@ public final class MathUtil {
public static double interpolate(double startValue, double endValue, double t) {
return startValue + (endValue - startValue) * MathUtil.clamp(t, 0, 1);
}
/**
* Checks if the given value matches an expected value within a certain tolerance.
*
* @param expected The expected value
* @param actual The actual value
* @param tolerance The allowed difference between the actual and the expected value
* @return Whether or not the actual value is within the allowed tolerance
*/
public static boolean isNear(double expected, double actual, double tolerance) {
if (tolerance < 0) {
throw new IllegalArgumentException("Tolerance must be a non-negative number!");
}
return Math.abs(expected - actual) < tolerance;
}
/**
* Checks if the given value matches an expected value within a certain tolerance. Supports
* continuous input for cases like absolute encoders.
*
* <p>Continuous input means that the min and max value are considered to be the same point, and
* tolerances can be checked across them. A common example would be for absolute encoders: calling
* isNear(2, 359, 5, 0, 360) returns true because 359 is 1 away from 360 (which is treated as the
* same as 0) and 2 is 2 away from 0, adding up to an error of 3 degrees, which is within the
* given tolerance of 5.
*
* @param expected The expected value
* @param actual The actual value
* @param tolerance The allowed difference between the actual and the expected value
* @param min Smallest value before wrapping around to the largest value
* @param max Largest value before wrapping around to the smallest value
* @return Whether or not the actual value is within the allowed tolerance
*/
public static boolean isNear(
double expected, double actual, double tolerance, double min, double max) {
if (tolerance < 0) {
throw new IllegalArgumentException("Tolerance must be a non-negative number!");
}
// Max error is exactly halfway between the min and max
double errorBound = (max - min) / 2.0;
double error = MathUtil.inputModulus(expected - actual, -errorBound, errorBound);
return Math.abs(error) < tolerance;
}
}

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

@@ -105,6 +105,58 @@ constexpr T InputModulus(T input, T minimumInput, T maximumInput) {
return input;
}
/**
* Checks if the given value matches an expected value within a certain
* tolerance.
*
* @param expected The expected value
* @param actual The actual value
* @param tolerance The allowed difference between the actual and the expected
* value
* @return Whether or not the actual value is within the allowed tolerance
*/
template <typename T>
requires std::is_arithmetic_v<T> || units::traits::is_unit_t_v<T>
constexpr bool IsNear(T expected, T actual, T tolerance) {
if constexpr (std::is_arithmetic_v<T>) {
return std::abs(expected - actual) < tolerance;
} else {
return units::math::abs(expected - actual) < tolerance;
}
}
/**
* Checks if the given value matches an expected value within a certain
* tolerance. Supports continuous input for cases like absolute encoders.
*
* Continuous input means that the min and max value are considered to be the
* same point, and tolerances can be checked across them. A common example
* would be for absolute encoders: calling isNear(2, 359, 5, 0, 360) returns
* true because 359 is 1 away from 360 (which is treated as the same as 0) and
* 2 is 2 away from 0, adding up to an error of 3 degrees, which is within the
* given tolerance of 5.
*
* @param expected The expected value
* @param actual The actual value
* @param tolerance The allowed difference between the actual and the expected
* value
* @param min Smallest value before wrapping around to the largest value
* @param max Largest value before wrapping around to the smallest value
* @return Whether or not the actual value is within the allowed tolerance
*/
template <typename T>
requires std::is_arithmetic_v<T> || units::traits::is_unit_t_v<T>
constexpr bool IsNear(T expected, T actual, T tolerance, T min, T max) {
T errorBound = (max - min) / 2.0;
T error = frc::InputModulus<T>(expected - actual, -errorBound, errorBound);
if constexpr (std::is_arithmetic_v<T>) {
return std::abs(error) < tolerance;
} else {
return units::math::abs(error) < tolerance;
}
}
/**
* Wraps an angle to the range -pi to pi radians (-180 to 180 degrees).
*

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

@@ -5,6 +5,8 @@
package edu.wpi.first.math;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertFalse;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.wpilibj.UtilityClassTest;
import org.junit.jupiter.api.Test;
@@ -93,4 +95,50 @@ class MathUtilTest extends UtilityClassTest<MathUtil> {
assertEquals(MathUtil.angleModulus(Math.PI / 2), Math.PI / 2);
assertEquals(MathUtil.angleModulus(-Math.PI / 2), -Math.PI / 2);
}
@Test
void testIsNear() {
// The answer is always 42
// Positive integer checks
assertTrue(MathUtil.isNear(42, 42, 1));
assertTrue(MathUtil.isNear(42, 41, 2));
assertTrue(MathUtil.isNear(42, 43, 2));
assertFalse(MathUtil.isNear(42, 44, 1));
// Negative integer checks
assertTrue(MathUtil.isNear(-42, -42, 1));
assertTrue(MathUtil.isNear(-42, -41, 2));
assertTrue(MathUtil.isNear(-42, -43, 2));
assertFalse(MathUtil.isNear(-42, -44, 1));
// Mixed sign integer checks
assertFalse(MathUtil.isNear(-42, 42, 1));
assertFalse(MathUtil.isNear(-42, 41, 2));
assertFalse(MathUtil.isNear(-42, 43, 2));
assertFalse(MathUtil.isNear(42, -42, 1));
assertFalse(MathUtil.isNear(42, -41, 2));
assertFalse(MathUtil.isNear(42, -43, 2));
// Floating point checks
assertTrue(MathUtil.isNear(42, 41.5, 1));
assertTrue(MathUtil.isNear(42, 42.5, 1));
assertTrue(MathUtil.isNear(42, 41.5, 0.75));
assertTrue(MathUtil.isNear(42, 42.5, 0.75));
// Wraparound checks
assertTrue(MathUtil.isNear(0, 356, 5, 0, 360));
assertTrue(MathUtil.isNear(0, -356, 5, 0, 360));
assertTrue(MathUtil.isNear(0, 4, 5, 0, 360));
assertTrue(MathUtil.isNear(0, -4, 5, 0, 360));
assertTrue(MathUtil.isNear(400, 41, 5, 0, 360));
assertTrue(MathUtil.isNear(400, -319, 5, 0, 360));
assertTrue(MathUtil.isNear(400, 401, 5, 0, 360));
assertFalse(MathUtil.isNear(0, 356, 2.5, 0, 360));
assertFalse(MathUtil.isNear(0, -356, 2.5, 0, 360));
assertFalse(MathUtil.isNear(0, 4, 2.5, 0, 360));
assertFalse(MathUtil.isNear(0, -4, 2.5, 0, 360));
assertFalse(MathUtil.isNear(400, 35, 5, 0, 360));
assertFalse(MathUtil.isNear(400, -315, 5, 0, 360));
assertFalse(MathUtil.isNear(400, 395, 5, 0, 360));
}
}

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

@@ -117,3 +117,49 @@ TEST(MathUtilTest, AngleModulus) {
EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t{-std::numbers::pi / 2}),
units::radian_t{-std::numbers::pi / 2});
}
TEST(MathUtilTest, IsNear) {
// The answer is always 42
// Positive integer checks
EXPECT_TRUE(frc::IsNear(42, 42, 1));
EXPECT_TRUE(frc::IsNear(42, 41, 2));
EXPECT_TRUE(frc::IsNear(42, 43, 2));
EXPECT_FALSE(frc::IsNear(42, 44, 1));
// Negative integer checks
EXPECT_TRUE(frc::IsNear(-42, -42, 1));
EXPECT_TRUE(frc::IsNear(-42, -41, 2));
EXPECT_TRUE(frc::IsNear(-42, -43, 2));
EXPECT_FALSE(frc::IsNear(-42, -44, 1));
// Mixed sign integer checks
EXPECT_FALSE(frc::IsNear(-42, 42, 1));
EXPECT_FALSE(frc::IsNear(-42, 41, 2));
EXPECT_FALSE(frc::IsNear(-42, 43, 2));
EXPECT_FALSE(frc::IsNear(42, -42, 1));
EXPECT_FALSE(frc::IsNear(42, -41, 2));
EXPECT_FALSE(frc::IsNear(42, -43, 2));
// Floating point checks
EXPECT_TRUE(frc::IsNear<double>(42, 41.5, 1));
EXPECT_TRUE(frc::IsNear<double>(42, 42.5, 1));
EXPECT_TRUE(frc::IsNear<double>(42, 41.5, 0.75));
EXPECT_TRUE(frc::IsNear<double>(42, 42.5, 0.75));
// Wraparound checks
EXPECT_TRUE(frc::IsNear(0_deg, 356_deg, 5_deg, 0_deg, 360_deg));
EXPECT_TRUE(frc::IsNear(0, -356, 5, 0, 360));
EXPECT_TRUE(frc::IsNear(0, 4, 5, 0, 360));
EXPECT_TRUE(frc::IsNear(0, -4, 5, 0, 360));
EXPECT_TRUE(frc::IsNear(400, 41, 5, 0, 360));
EXPECT_TRUE(frc::IsNear(400, -319, 5, 0, 360));
EXPECT_TRUE(frc::IsNear(400, 401, 5, 0, 360));
EXPECT_FALSE(frc::IsNear<double>(0, 356, 2.5, 0, 360));
EXPECT_FALSE(frc::IsNear<double>(0, -356, 2.5, 0, 360));
EXPECT_FALSE(frc::IsNear<double>(0, 4, 2.5, 0, 360));
EXPECT_FALSE(frc::IsNear<double>(0, -4, 2.5, 0, 360));
EXPECT_FALSE(frc::IsNear(400, 35, 5, 0, 360));
EXPECT_FALSE(frc::IsNear(400, -315, 5, 0, 360));
EXPECT_FALSE(frc::IsNear(400, 395, 5, 0, 360));
EXPECT_FALSE(frc::IsNear(0_deg, -4_deg, 2.5_deg, 0_deg, 360_deg));
}