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] Move RobotDriveBase::ApplyDeadband() to MathUtil (#3529)

Browse Source 
It's a useful function outside of the drive classes.

For backwards compatibility, deprecate (rather than remove) RobotDriveBase.applyDeadband()
This commit is contained in:
Tyler Veness
2021-08-28 20:52:05 -07:00
committed by GitHub
parent 3b5d0d141a
commit aa3848b2c8
16 changed files with 162 additions and 65 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -7,57 +7,79 @@
#include "units/angle.h"
#define EXPECT_UNITS_EQ(a, b) \
EXPECT_FLOAT_EQ((a).to<double>(), (b).to<double>())
EXPECT_DOUBLE_EQ((a).to<double>(), (b).to<double>())
#define EXPECT_UNITS_NEAR(a, b, c) \
EXPECT_NEAR((a).to<double>(), (b).to<double>(), c)
TEST(MathUtilTest, ApplyDeadband) {
// < 0
EXPECT_DOUBLE_EQ(-1.0, frc::ApplyDeadband(-1.0, 0.02));
EXPECT_DOUBLE_EQ((-0.03 + 0.02) / (1.0 - 0.02),
frc::ApplyDeadband(-0.03, 0.02));
EXPECT_DOUBLE_EQ(0.0, frc::ApplyDeadband(-0.02, 0.02));
EXPECT_DOUBLE_EQ(0.0, frc::ApplyDeadband(-0.01, 0.02));
// == 0
EXPECT_DOUBLE_EQ(0.0, frc::ApplyDeadband(0.0, 0.02));
// > 0
EXPECT_DOUBLE_EQ(0.0, frc::ApplyDeadband(0.01, 0.02));
EXPECT_DOUBLE_EQ(0.0, frc::ApplyDeadband(0.02, 0.02));
EXPECT_DOUBLE_EQ((0.03 - 0.02) / (1.0 - 0.02),
frc::ApplyDeadband(0.03, 0.02));
EXPECT_DOUBLE_EQ(1.0, frc::ApplyDeadband(1.0, 0.02));
}
TEST(MathUtilTest, InputModulus) {
// These tests check error wrapping. That is, the result of wrapping the
// result of an angle reference minus the measurement.
// Test symmetric range
EXPECT_FLOAT_EQ(-20.0, frc::InputModulus(170.0 - (-170.0), -180.0, 180.0));
EXPECT_FLOAT_EQ(-20.0,
frc::InputModulus(170.0 + 360.0 - (-170.0), -180.0, 180.0));
EXPECT_FLOAT_EQ(-20.0,
frc::InputModulus(170.0 - (-170.0 + 360.0), -180.0, 180.0));
EXPECT_FLOAT_EQ(20.0, frc::InputModulus(-170.0 - 170.0, -180.0, 180.0));
EXPECT_FLOAT_EQ(20.0,
frc::InputModulus(-170.0 + 360.0 - 170.0, -180.0, 180.0));
EXPECT_FLOAT_EQ(20.0,
frc::InputModulus(-170.0 - (170.0 + 360.0), -180.0, 180.0));
EXPECT_DOUBLE_EQ(-20.0, frc::InputModulus(170.0 - (-170.0), -180.0, 180.0));
EXPECT_DOUBLE_EQ(-20.0,
frc::InputModulus(170.0 + 360.0 - (-170.0), -180.0, 180.0));
EXPECT_DOUBLE_EQ(-20.0,
frc::InputModulus(170.0 - (-170.0 + 360.0), -180.0, 180.0));
EXPECT_DOUBLE_EQ(20.0, frc::InputModulus(-170.0 - 170.0, -180.0, 180.0));
EXPECT_DOUBLE_EQ(20.0,
frc::InputModulus(-170.0 + 360.0 - 170.0, -180.0, 180.0));
EXPECT_DOUBLE_EQ(20.0,
frc::InputModulus(-170.0 - (170.0 + 360.0), -180.0, 180.0));
// Test range starting at zero
EXPECT_FLOAT_EQ(340.0, frc::InputModulus(170.0 - 190.0, 0.0, 360.0));
EXPECT_FLOAT_EQ(340.0, frc::InputModulus(170.0 + 360.0 - 190.0, 0.0, 360.0));
EXPECT_FLOAT_EQ(340.0,
frc::InputModulus(170.0 - (190.0 + 360.0), 0.0, 360.0));
EXPECT_DOUBLE_EQ(340.0, frc::InputModulus(170.0 - 190.0, 0.0, 360.0));
EXPECT_DOUBLE_EQ(340.0, frc::InputModulus(170.0 + 360.0 - 190.0, 0.0, 360.0));
EXPECT_DOUBLE_EQ(340.0,
frc::InputModulus(170.0 - (190.0 + 360.0), 0.0, 360.0));
// Test asymmetric range that doesn't start at zero
EXPECT_FLOAT_EQ(-20.0, frc::InputModulus(170.0 - (-170.0), -170.0, 190.0));
EXPECT_DOUBLE_EQ(-20.0, frc::InputModulus(170.0 - (-170.0), -170.0, 190.0));
// Test range with both positive endpoints
EXPECT_FLOAT_EQ(2.0, frc::InputModulus(0.0, 1.0, 3.0));
EXPECT_FLOAT_EQ(3.0, frc::InputModulus(1.0, 1.0, 3.0));
EXPECT_FLOAT_EQ(2.0, frc::InputModulus(2.0, 1.0, 3.0));
EXPECT_FLOAT_EQ(3.0, frc::InputModulus(3.0, 1.0, 3.0));
EXPECT_FLOAT_EQ(2.0, frc::InputModulus(4.0, 1.0, 3.0));
EXPECT_DOUBLE_EQ(2.0, frc::InputModulus(0.0, 1.0, 3.0));
EXPECT_DOUBLE_EQ(3.0, frc::InputModulus(1.0, 1.0, 3.0));
EXPECT_DOUBLE_EQ(2.0, frc::InputModulus(2.0, 1.0, 3.0));
EXPECT_DOUBLE_EQ(3.0, frc::InputModulus(3.0, 1.0, 3.0));
EXPECT_DOUBLE_EQ(2.0, frc::InputModulus(4.0, 1.0, 3.0));
// Test all supported types
EXPECT_FLOAT_EQ(-20.0,
frc::InputModulus<double>(170.0 - (-170.0), -170.0, 190.0));
EXPECT_DOUBLE_EQ(-20.0,
frc::InputModulus<double>(170.0 - (-170.0), -170.0, 190.0));
EXPECT_EQ(-20, frc::InputModulus<int>(170 - (-170), -170, 190));
EXPECT_EQ(-20_deg, frc::InputModulus<units::degree_t>(170_deg - (-170_deg),
-170_deg, 190_deg));
}
TEST(MathUtilTest, AngleModulus) {
EXPECT_UNITS_EQ(
EXPECT_UNITS_NEAR(
frc::AngleModulus(units::radian_t{-2000 * wpi::numbers::pi / 180}),
units::radian_t{160 * wpi::numbers::pi / 180});
EXPECT_UNITS_EQ(
units::radian_t{160 * wpi::numbers::pi / 180}, 1e-10);
EXPECT_UNITS_NEAR(
frc::AngleModulus(units::radian_t{358 * wpi::numbers::pi / 180}),
units::radian_t{-2 * wpi::numbers::pi / 180});
EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t{2.0 * wpi::numbers::pi}),
0_rad);
units::radian_t{-2 * wpi::numbers::pi / 180}, 1e-10);
EXPECT_UNITS_NEAR(frc::AngleModulus(units::radian_t{2.0 * wpi::numbers::pi}),
0_rad, 1e-10);
EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t(5 * wpi::numbers::pi)),
units::radian_t(wpi::numbers::pi));