From aa3848b2c89a2ffa6f039aa5bd826adbfd04f642 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sat, 28 Aug 2021 20:52:05 -0700 Subject: [PATCH] [wpimath] Move RobotDriveBase::ApplyDeadband() to MathUtil (#3529) It's a useful function outside of the drive classes. For backwards compatibility, deprecate (rather than remove) RobotDriveBase.applyDeadband() --- .../native/cpp/drive/DifferentialDrive.cpp | 1 + .../main/native/cpp/drive/KilloughDrive.cpp | 1 + .../main/native/cpp/drive/MecanumDrive.cpp | 1 + .../main/native/cpp/drive/RobotDriveBase.cpp | 11 +-- .../native/include/frc/drive/RobotDriveBase.h | 5 +- .../main/cpp/examples/SwerveBot/cpp/Robot.cpp | 10 ++- .../wpilibj/drive/DifferentialDrive.java | 12 +-- .../first/wpilibj/drive/KilloughDrive.java | 4 +- .../wpi/first/wpilibj/drive/MecanumDrive.java | 4 +- .../first/wpilibj/drive/RobotDriveBase.java | 15 ++-- .../wpilibj/examples/swervebot/Robot.java | 13 ++- .../java/edu/wpi/first/math/MathUtil.java | 20 +++++ wpimath/src/main/native/cpp/MathUtil.cpp | 23 ++++++ .../src/main/native/include/frc/MathUtil.h | 9 +++ .../java/edu/wpi/first/math/MathUtilTest.java | 18 +++++ wpimath/src/test/native/cpp/MathUtilTest.cpp | 80 ++++++++++++------- 16 files changed, 162 insertions(+), 65 deletions(-) create mode 100644 wpimath/src/main/native/cpp/MathUtil.cpp diff --git a/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp b/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp index 7cbb17383a..b54b607720 100644 --- a/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp +++ b/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp @@ -11,6 +11,7 @@ #include #include +#include "frc/MathUtil.h" #include "frc/SpeedController.h" using namespace frc; diff --git a/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp b/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp index 125157007b..c84767801b 100644 --- a/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp +++ b/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp @@ -12,6 +12,7 @@ #include #include +#include "frc/MathUtil.h" #include "frc/SpeedController.h" using namespace frc; diff --git a/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp b/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp index e6141df06e..b74c6113a0 100644 --- a/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp +++ b/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp @@ -12,6 +12,7 @@ #include #include +#include "frc/MathUtil.h" #include "frc/SpeedController.h" #include "frc/drive/Vector2d.h" diff --git a/wpilibc/src/main/native/cpp/drive/RobotDriveBase.cpp b/wpilibc/src/main/native/cpp/drive/RobotDriveBase.cpp index 5dcd2876c1..215995840e 100644 --- a/wpilibc/src/main/native/cpp/drive/RobotDriveBase.cpp +++ b/wpilibc/src/main/native/cpp/drive/RobotDriveBase.cpp @@ -10,6 +10,7 @@ #include +#include "frc/MathUtil.h" #include "frc/motorcontrol/MotorController.h" using namespace frc; @@ -31,15 +32,7 @@ void RobotDriveBase::FeedWatchdog() { } double RobotDriveBase::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; - } + return frc::ApplyDeadband(value, deadband); } void RobotDriveBase::Normalize(wpi::span wheelSpeeds) { diff --git a/wpilibc/src/main/native/include/frc/drive/RobotDriveBase.h b/wpilibc/src/main/native/include/frc/drive/RobotDriveBase.h index 87003d5bf1..16f0961fc8 100644 --- a/wpilibc/src/main/native/include/frc/drive/RobotDriveBase.h +++ b/wpilibc/src/main/native/include/frc/drive/RobotDriveBase.h @@ -7,6 +7,7 @@ #include #include +#include #include #include "frc/MotorSafety.h" @@ -42,7 +43,7 @@ class RobotDriveBase : public MotorSafety { * * The default value is 0.02. Inputs smaller than the deadband are set to 0.0 * while inputs larger than the deadband are scaled from 0.0 to 1.0. See - * ApplyDeadband(). + * frc::ApplyDeadband(). * * @param deadband The deadband to set. */ @@ -75,7 +76,9 @@ class RobotDriveBase : public MotorSafety { * * @param value value to clip * @param deadband range around zero + * @deprecated Use ApplyDeadband() in frc/MathUtil.h. */ + WPI_DEPRECATED("Use ApplyDeadband() in frc/MathUtil.h") static double ApplyDeadband(double number, double deadband); /** diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Robot.cpp index 3f5f6751b3..39cfa14639 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Robot.cpp @@ -2,6 +2,7 @@ // 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 #include #include #include @@ -30,20 +31,23 @@ class Robot : public frc::TimedRobot { void DriveWithJoystick(bool fieldRelative) { // Get the x speed. We are inverting this because Xbox controllers return // negative values when we push forward. - const auto xSpeed = -m_xspeedLimiter.Calculate(m_controller.GetLeftY()) * + const auto xSpeed = -m_xspeedLimiter.Calculate( + frc::ApplyDeadband(m_controller.GetLeftY(), 0.02)) * Drivetrain::kMaxSpeed; // Get the y speed or sideways/strafe speed. We are inverting this because // we want a positive value when we pull to the left. Xbox controllers // return positive values when you pull to the right by default. - const auto ySpeed = -m_yspeedLimiter.Calculate(m_controller.GetLeftX()) * + const auto ySpeed = -m_yspeedLimiter.Calculate( + frc::ApplyDeadband(m_controller.GetLeftX(), 0.02)) * Drivetrain::kMaxSpeed; // Get the rate of angular rotation. We are inverting this because we want a // positive value when we pull to the left (remember, CCW is positive in // mathematics). Xbox controllers return positive values when you pull to // the right by default. - const auto rot = -m_rotLimiter.Calculate(m_controller.GetRightX()) * + const auto rot = -m_rotLimiter.Calculate( + frc::ApplyDeadband(m_controller.GetRightX(), 0.02)) * Drivetrain::kMaxAngularSpeed; m_swerve.Drive(xSpeed, ySpeed, rot, fieldRelative); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java index 1d94b44f54..63305936f6 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java @@ -170,8 +170,8 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC m_reported = true; } - xSpeed = applyDeadband(xSpeed, m_deadband); - zRotation = applyDeadband(zRotation, m_deadband); + xSpeed = MathUtil.applyDeadband(xSpeed, m_deadband); + zRotation = MathUtil.applyDeadband(zRotation, m_deadband); var speeds = arcadeDriveIK(xSpeed, zRotation, squareInputs); @@ -203,8 +203,8 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC m_reported = true; } - xSpeed = applyDeadband(xSpeed, m_deadband); - zRotation = applyDeadband(zRotation, m_deadband); + xSpeed = MathUtil.applyDeadband(xSpeed, m_deadband); + zRotation = MathUtil.applyDeadband(zRotation, m_deadband); var speeds = curvatureDriveIK(xSpeed, zRotation, allowTurnInPlace); @@ -241,8 +241,8 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC m_reported = true; } - leftSpeed = applyDeadband(leftSpeed, m_deadband); - rightSpeed = applyDeadband(rightSpeed, m_deadband); + leftSpeed = MathUtil.applyDeadband(leftSpeed, m_deadband); + rightSpeed = MathUtil.applyDeadband(rightSpeed, m_deadband); var speeds = tankDriveIK(leftSpeed, rightSpeed, squareInputs); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/KilloughDrive.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/KilloughDrive.java index 9b629dc373..3c49989fec 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/KilloughDrive.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/KilloughDrive.java @@ -191,8 +191,8 @@ public class KilloughDrive extends RobotDriveBase implements Sendable, AutoClose m_reported = true; } - ySpeed = applyDeadband(ySpeed, m_deadband); - xSpeed = applyDeadband(xSpeed, m_deadband); + ySpeed = MathUtil.applyDeadband(ySpeed, m_deadband); + xSpeed = MathUtil.applyDeadband(xSpeed, m_deadband); var speeds = driveCartesianIK(ySpeed, xSpeed, zRotation, gyroAngle); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java index 637d83e8e9..831dba366b 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java @@ -169,8 +169,8 @@ public class MecanumDrive extends RobotDriveBase implements Sendable, AutoClosea m_reported = true; } - ySpeed = applyDeadband(ySpeed, m_deadband); - xSpeed = applyDeadband(xSpeed, m_deadband); + ySpeed = MathUtil.applyDeadband(ySpeed, m_deadband); + xSpeed = MathUtil.applyDeadband(xSpeed, m_deadband); var speeds = driveCartesianIK(ySpeed, xSpeed, zRotation, gyroAngle); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/RobotDriveBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/RobotDriveBase.java index 03ded74912..569f94c8f5 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/RobotDriveBase.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/RobotDriveBase.java @@ -4,6 +4,7 @@ package edu.wpi.first.wpilibj.drive; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.MotorSafety; /** Common base class for drive platforms. */ @@ -41,7 +42,7 @@ public abstract class RobotDriveBase extends MotorSafety { * *

The default value is {@value #kDefaultDeadband}. Inputs smaller than the deadband are set to * 0.0 while inputs larger than the deadband are scaled from 0.0 to 1.0. See {@link - * #applyDeadband}. + * edu.wpi.first.math.MathUtil#applyDeadband}. * * @param deadband The deadband to set. */ @@ -83,17 +84,11 @@ public abstract class RobotDriveBase extends MotorSafety { * @param value value to clip * @param deadband range around zero * @return The value after the deadband is applied. + * @deprecated Use MathUtil.applyDeadband(double,double). */ + @Deprecated(since = "2021", forRemoval = true) protected 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 MathUtil.applyDeadband(value, deadband); } /** diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Robot.java index 129a936b2d..b2d39eff35 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Robot.java @@ -4,6 +4,7 @@ package edu.wpi.first.wpilibj.examples.swervebot; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.XboxController; @@ -31,18 +32,24 @@ public class Robot extends TimedRobot { private void driveWithJoystick(boolean fieldRelative) { // Get the x speed. We are inverting this because Xbox controllers return // negative values when we push forward. - final var xSpeed = -m_xspeedLimiter.calculate(m_controller.getLeftY()) * Drivetrain.kMaxSpeed; + final var xSpeed = + -m_xspeedLimiter.calculate(MathUtil.applyDeadband(m_controller.getLeftY(), 0.02)) + * Drivetrain.kMaxSpeed; // Get the y speed or sideways/strafe speed. We are inverting this because // we want a positive value when we pull to the left. Xbox controllers // return positive values when you pull to the right by default. - final var ySpeed = -m_yspeedLimiter.calculate(m_controller.getLeftX()) * Drivetrain.kMaxSpeed; + final var ySpeed = + -m_yspeedLimiter.calculate(MathUtil.applyDeadband(m_controller.getLeftX(), 0.02)) + * Drivetrain.kMaxSpeed; // Get the rate of angular rotation. We are inverting this because we want a // positive value when we pull to the left (remember, CCW is positive in // mathematics). Xbox controllers return positive values when you pull to // the right by default. - final var rot = -m_rotLimiter.calculate(m_controller.getRightX()) * Drivetrain.kMaxAngularSpeed; + final var rot = + -m_rotLimiter.calculate(MathUtil.applyDeadband(m_controller.getRightX(), 0.02)) + * Drivetrain.kMaxAngularSpeed; m_swerve.drive(xSpeed, ySpeed, rot, fieldRelative); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java b/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java index ad9ff35bee..791de8a242 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java +++ b/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java @@ -33,6 +33,26 @@ 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 1.0 is scaled from 0.0 to 1.0. + * + * @param value Value to clip. + * @param deadband Range around zero. + * @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; + } + } + /** * Returns modulus of input. * diff --git a/wpimath/src/main/native/cpp/MathUtil.cpp b/wpimath/src/main/native/cpp/MathUtil.cpp new file mode 100644 index 0000000000..19cd214ead --- /dev/null +++ b/wpimath/src/main/native/cpp/MathUtil.cpp @@ -0,0 +1,23 @@ +// 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 + +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 diff --git a/wpimath/src/main/native/include/frc/MathUtil.h b/wpimath/src/main/native/include/frc/MathUtil.h index 2cef244e57..3ea753da2a 100644 --- a/wpimath/src/main/native/include/frc/MathUtil.h +++ b/wpimath/src/main/native/include/frc/MathUtil.h @@ -10,6 +10,15 @@ 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. + * + * @param value Value to clip. + * @param deadband Range around zero. + */ +double ApplyDeadband(double value, double deadband); + /** * Returns modulus of input. * diff --git a/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java b/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java index af48bad955..bb116cee7a 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java @@ -9,6 +9,24 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import org.junit.jupiter.api.Test; class MathUtilTest { + @Test + void testApplyDeadband() { + // < 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)); + assertEquals(0.0, MathUtil.applyDeadband(-0.02, 0.02)); + assertEquals(0.0, MathUtil.applyDeadband(-0.01, 0.02)); + + // == 0 + assertEquals(0.0, MathUtil.applyDeadband(0.0, 0.02)); + + // > 0 + assertEquals(0.0, MathUtil.applyDeadband(0.01, 0.02)); + assertEquals(0.0, MathUtil.applyDeadband(0.02, 0.02)); + assertEquals((0.03 - 0.02) / (1.0 - 0.02), MathUtil.applyDeadband(0.03, 0.02)); + assertEquals(1.0, MathUtil.applyDeadband(1.0, 0.02)); + } + @Test void testInputModulus() { // These tests check error wrapping. That is, the result of wrapping the diff --git a/wpimath/src/test/native/cpp/MathUtilTest.cpp b/wpimath/src/test/native/cpp/MathUtilTest.cpp index 8ef4f0b455..23c85542f1 100644 --- a/wpimath/src/test/native/cpp/MathUtilTest.cpp +++ b/wpimath/src/test/native/cpp/MathUtilTest.cpp @@ -7,57 +7,79 @@ #include "units/angle.h" #define EXPECT_UNITS_EQ(a, b) \ - EXPECT_FLOAT_EQ((a).to(), (b).to()) + EXPECT_DOUBLE_EQ((a).to(), (b).to()) + +#define EXPECT_UNITS_NEAR(a, b, c) \ + EXPECT_NEAR((a).to(), (b).to(), 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(170.0 - (-170.0), -170.0, 190.0)); + EXPECT_DOUBLE_EQ(-20.0, + frc::InputModulus(170.0 - (-170.0), -170.0, 190.0)); EXPECT_EQ(-20, frc::InputModulus(170 - (-170), -170, 190)); EXPECT_EQ(-20_deg, frc::InputModulus(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));