diff --git a/wpilibc/src/main/native/cpp/controller/PIDController.cpp b/wpilibc/src/main/native/cpp/controller/PIDController.cpp index a0087ba668..566d36f0d0 100644 --- a/wpilibc/src/main/native/cpp/controller/PIDController.cpp +++ b/wpilibc/src/main/native/cpp/controller/PIDController.cpp @@ -9,7 +9,7 @@ #include -#include "frc/controller/ControllerUtil.h" +#include "frc/MathUtil.h" #include "frc/smartdashboard/SendableBuilder.h" #include "frc/smartdashboard/SendableRegistry.h" @@ -69,8 +69,8 @@ double PIDController::GetSetpoint() const { bool PIDController::AtSetpoint() const { double positionError; if (m_continuous) { - positionError = frc::GetModulusError( - m_setpoint, m_measurement, m_minimumInput, m_maximumInput); + positionError = frc::InputModulus(m_setpoint - m_measurement, + m_minimumInput, m_maximumInput); } else { positionError = m_setpoint - m_measurement; } @@ -121,8 +121,8 @@ double PIDController::Calculate(double measurement) { m_prevError = m_positionError; if (m_continuous) { - m_positionError = frc::GetModulusError( - m_setpoint, measurement, m_minimumInput, m_maximumInput); + m_positionError = frc::InputModulus(m_setpoint - measurement, + m_minimumInput, m_maximumInput); } else { m_positionError = m_setpoint - measurement; } diff --git a/wpilibc/src/main/native/include/frc/controller/ControllerUtil.h b/wpilibc/src/main/native/include/frc/controller/ControllerUtil.h deleted file mode 100644 index cdba1a9798..0000000000 --- a/wpilibc/src/main/native/include/frc/controller/ControllerUtil.h +++ /dev/null @@ -1,39 +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. - -#pragma once - -#include -#include - -#include - -namespace frc { - -/** - * Returns modulus of error where error is the difference between the reference - * and a measurement. - * - * @param reference Reference input of a controller. - * @param measurement The current measurement. - * @param minimumInput The minimum value expected from the input. - * @param maximumInput The maximum value expected from the input. - */ -template -T GetModulusError(T reference, T measurement, T minimumInput, T maximumInput) { - T error = reference - measurement; - T modulus = maximumInput - minimumInput; - - // Wrap error above maximum input - int numMax = (error + maximumInput) / modulus; - error -= numMax * modulus; - - // Wrap error below minimum input - int numMin = (error + minimumInput) / modulus; - error -= numMin * modulus; - - return error; -} - -} // namespace frc diff --git a/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h b/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h index 864f770f43..e6733e37a4 100644 --- a/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h +++ b/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h @@ -11,7 +11,7 @@ #include -#include "frc/controller/ControllerUtil.h" +#include "frc/MathUtil.h" #include "frc/controller/PIDController.h" #include "frc/smartdashboard/Sendable.h" #include "frc/smartdashboard/SendableBuilder.h" @@ -253,10 +253,10 @@ class ProfiledPIDController double Calculate(Distance_t measurement) { if (m_controller.IsContinuousInputEnabled()) { // Get error which is smallest distance between goal and measurement - auto goalMinDistance = frc::GetModulusError( - m_goal.position, measurement, m_minimumInput, m_maximumInput); - auto setpointMinDistance = frc::GetModulusError( - m_setpoint.position, measurement, m_minimumInput, m_maximumInput); + auto goalMinDistance = frc::InputModulus( + m_goal.position - measurement, m_minimumInput, m_maximumInput); + auto setpointMinDistance = frc::InputModulus( + m_setpoint.position - measurement, m_minimumInput, m_maximumInput); // Recompute the profile goal with the smallest error, thus giving the // shortest path. The goal may be outside the input range after this diff --git a/wpilibc/src/test/native/cpp/controller/ControllerUtilTest.cpp b/wpilibc/src/test/native/cpp/controller/ControllerUtilTest.cpp deleted file mode 100644 index e9cb7caa95..0000000000 --- a/wpilibc/src/test/native/cpp/controller/ControllerUtilTest.cpp +++ /dev/null @@ -1,39 +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 - -#include "frc/controller/ControllerUtil.h" -#include "gtest/gtest.h" - -TEST(ControllerUtilTest, GetModulusError) { - // Test symmetric range - EXPECT_FLOAT_EQ(-20.0, frc::GetModulusError(170.0, -170.0, -180.0, 180.0)); - EXPECT_FLOAT_EQ(-20.0, - frc::GetModulusError(170.0 + 360.0, -170.0, -180.0, 180.0)); - EXPECT_FLOAT_EQ(-20.0, - frc::GetModulusError(170.0, -170.0 + 360.0, -180.0, 180.0)); - EXPECT_FLOAT_EQ(20.0, frc::GetModulusError(-170.0, 170.0, -180.0, 180.0)); - EXPECT_FLOAT_EQ(20.0, - frc::GetModulusError(-170.0 + 360.0, 170.0, -180.0, 180.0)); - EXPECT_FLOAT_EQ(20.0, - frc::GetModulusError(-170.0, 170.0 + 360.0, -180.0, 180.0)); - - // Test range starting at zero - EXPECT_FLOAT_EQ(-20.0, frc::GetModulusError(170.0, 190.0, 0.0, 360.0)); - EXPECT_FLOAT_EQ(-20.0, - frc::GetModulusError(170.0 + 360.0, 190.0, 0.0, 360.0)); - EXPECT_FLOAT_EQ(-20.0, - frc::GetModulusError(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::GetModulusError(170.0, -170.0, -170.0, 190.0)); - - // Test all supported types - EXPECT_FLOAT_EQ(-20.0, - frc::GetModulusError(170.0, -170.0, -170.0, 190.0)); - EXPECT_EQ(-20, frc::GetModulusError(170, -170, -170, 190)); - EXPECT_EQ(-20_deg, frc::GetModulusError(170_deg, -170_deg, - -170_deg, 190_deg)); -} diff --git a/wpilibc/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp b/wpilibc/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp index c5ee66fbed..d12eaf76cb 100644 --- a/wpilibc/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp +++ b/wpilibc/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp @@ -6,6 +6,7 @@ #include #include +#include "frc/MathUtil.h" #include "frc/controller/HolonomicDriveController.h" #include "frc/trajectory/TrajectoryGenerator.h" #include "gtest/gtest.h" @@ -43,6 +44,6 @@ TEST(HolonomicDriveControllerTest, ReachesReference) { auto& endPose = trajectory.States().back().pose; EXPECT_NEAR_UNITS(endPose.X(), robotPose.X(), kTolerance); EXPECT_NEAR_UNITS(endPose.Y(), robotPose.Y(), kTolerance); - EXPECT_NEAR_UNITS(units::math::NormalizeAngle(robotPose.Rotation().Radians()), - 0_rad, kAngularTolerance); + EXPECT_NEAR_UNITS(frc::AngleModulus(robotPose.Rotation().Radians()), 0_rad, + kAngularTolerance); } diff --git a/wpilibc/src/test/native/cpp/controller/RamseteControllerTest.cpp b/wpilibc/src/test/native/cpp/controller/RamseteControllerTest.cpp index 3a3bc8a707..e0c5100d8d 100644 --- a/wpilibc/src/test/native/cpp/controller/RamseteControllerTest.cpp +++ b/wpilibc/src/test/native/cpp/controller/RamseteControllerTest.cpp @@ -4,6 +4,7 @@ #include +#include "frc/MathUtil.h" #include "frc/controller/RamseteController.h" #include "frc/trajectory/TrajectoryGenerator.h" #include "gtest/gtest.h" @@ -36,7 +37,7 @@ TEST(RamseteControllerTest, ReachesReference) { auto& endPose = trajectory.States().back().pose; EXPECT_NEAR_UNITS(endPose.X(), robotPose.X(), kTolerance); EXPECT_NEAR_UNITS(endPose.Y(), robotPose.Y(), kTolerance); - EXPECT_NEAR_UNITS(units::math::NormalizeAngle(endPose.Rotation().Radians() - - robotPose.Rotation().Radians()), + EXPECT_NEAR_UNITS(frc::AngleModulus(endPose.Rotation().Radians() - + robotPose.Rotation().Radians()), 0_rad, kAngularTolerance); } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java index 007f18cc90..d83f58413b 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java @@ -201,7 +201,7 @@ public class PIDController implements Sendable, AutoCloseable { double positionError; if (m_continuous) { positionError = - ControllerUtil.getModulusError(m_setpoint, m_measurement, m_minimumInput, m_maximumInput); + MathUtil.inputModulus(m_setpoint - m_measurement, m_minimumInput, m_maximumInput); } else { positionError = m_setpoint - m_measurement; } @@ -308,7 +308,7 @@ public class PIDController implements Sendable, AutoCloseable { if (m_continuous) { m_positionError = - ControllerUtil.getModulusError(m_setpoint, measurement, m_minimumInput, m_maximumInput); + MathUtil.inputModulus(m_setpoint - measurement, m_minimumInput, m_maximumInput); } else { m_positionError = m_setpoint - measurement; } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ProfiledPIDController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ProfiledPIDController.java index 1487cd5e66..427291875b 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ProfiledPIDController.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ProfiledPIDController.java @@ -9,6 +9,7 @@ import edu.wpi.first.hal.HAL; import edu.wpi.first.wpilibj.Sendable; import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile; +import edu.wpi.first.wpiutil.math.MathUtil; /** * Implements a PID control loop whose setpoint is constrained by a trapezoid profile. Users should @@ -270,11 +271,9 @@ public class ProfiledPIDController implements Sendable { if (m_controller.isContinuousInputEnabled()) { // Get error which is smallest distance between goal and measurement double goalMinDistance = - ControllerUtil.getModulusError( - m_goal.position, measurement, m_minimumInput, m_maximumInput); + MathUtil.inputModulus(m_goal.position - measurement, m_minimumInput, m_maximumInput); double setpointMinDistance = - ControllerUtil.getModulusError( - m_setpoint.position, measurement, m_minimumInput, m_maximumInput); + MathUtil.inputModulus(m_setpoint.position - measurement, m_minimumInput, m_maximumInput); // Recompute the profile goal with the smallest error, thus giving the shortest path. The goal // may be outside the input range after this operation, but that's OK because the controller diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ControllerUtilTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ControllerUtilTest.java deleted file mode 100644 index 853621b653..0000000000 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ControllerUtilTest.java +++ /dev/null @@ -1,30 +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. - -package edu.wpi.first.wpilibj.controller; - -import static org.junit.jupiter.api.Assertions.assertEquals; - -import org.junit.jupiter.api.Test; - -class ControllerUtilTest { - @Test - void testGetModulusError() { - // Test symmetric range - assertEquals(-20.0, ControllerUtil.getModulusError(170.0, -170.0, -180.0, 180.0)); - assertEquals(-20.0, ControllerUtil.getModulusError(170.0 + 360.0, -170.0, -180.0, 180.0)); - assertEquals(-20.0, ControllerUtil.getModulusError(170.0, -170.0 + 360.0, -180.0, 180.0)); - assertEquals(20.0, ControllerUtil.getModulusError(-170.0, 170.0, -180.0, 180.0)); - assertEquals(20.0, ControllerUtil.getModulusError(-170.0 + 360.0, 170.0, -180.0, 180.0)); - assertEquals(20.0, ControllerUtil.getModulusError(-170.0, 170.0 + 360.0, -180.0, 180.0)); - - // Test range start at zero - assertEquals(-20.0, ControllerUtil.getModulusError(170.0, 190.0, 0.0, 360.0)); - assertEquals(-20.0, ControllerUtil.getModulusError(170.0 + 360.0, 190.0, 0.0, 360.0)); - assertEquals(-20.0, ControllerUtil.getModulusError(170.0, 190.0 + 360, 0.0, 360.0)); - - // Test asymmetric range that doesn't start at zero - assertEquals(-20.0, ControllerUtil.getModulusError(170.0, -170.0, -170.0, 190.0)); - } -} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/HolonomicDriveControllerTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/HolonomicDriveControllerTest.java index d025a445bf..e63b6b1c25 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/HolonomicDriveControllerTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/HolonomicDriveControllerTest.java @@ -69,7 +69,7 @@ class HolonomicDriveControllerTest { () -> assertEquals( 0.0, - MathUtil.normalizeAngle(finalRobotPose.getRotation().getRadians()), + MathUtil.angleModulus(finalRobotPose.getRotation().getRadians()), kAngularTolerance)); } } diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/RamseteControllerTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/RamseteControllerTest.java index af188f363d..5800fdc0c1 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/RamseteControllerTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/RamseteControllerTest.java @@ -55,7 +55,7 @@ class RamseteControllerTest { () -> assertEquals( 0.0, - MathUtil.normalizeAngle( + MathUtil.angleModulus( endPose.getRotation().getRadians() - finalRobotPose.getRotation().getRadians()), kAngularTolerance)); } diff --git a/wpimath/src/dev/java/edu/wpi/first/wpiutil/math/DevMain.java b/wpimath/src/dev/java/edu/wpi/first/wpiutil/math/DevMain.java index 9bb99253fc..b2a8254917 100644 --- a/wpimath/src/dev/java/edu/wpi/first/wpiutil/math/DevMain.java +++ b/wpimath/src/dev/java/edu/wpi/first/wpiutil/math/DevMain.java @@ -8,7 +8,7 @@ public final class DevMain { /** Main entry point. */ public static void main(String[] args) { System.out.println("Hello World!"); - System.out.println(MathUtil.normalizeAngle(-5.0)); + System.out.println(MathUtil.angleModulus(-5.0)); } private DevMain() {} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ControllerUtil.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/ControllerUtil.java similarity index 100% rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ControllerUtil.java rename to wpimath/src/main/java/edu/wpi/first/wpilibj/controller/ControllerUtil.java diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/AngleStatistics.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/AngleStatistics.java index 7a73396564..c7077108bf 100644 --- a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/AngleStatistics.java +++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/AngleStatistics.java @@ -4,6 +4,7 @@ package edu.wpi.first.wpilibj.estimator; +import edu.wpi.first.wpiutil.math.MathUtil; import edu.wpi.first.wpiutil.math.Matrix; import edu.wpi.first.wpiutil.math.Num; import edu.wpi.first.wpiutil.math.numbers.N1; @@ -26,7 +27,7 @@ public final class AngleStatistics { public static Matrix angleResidual( Matrix a, Matrix b, int angleStateIdx) { Matrix ret = a.minus(b); - ret.set(angleStateIdx, 0, normalizeAngle(ret.get(angleStateIdx, 0))); + ret.set(angleStateIdx, 0, MathUtil.angleModulus(ret.get(angleStateIdx, 0))); return ret; } @@ -52,7 +53,7 @@ public final class AngleStatistics { public static Matrix angleAdd( Matrix a, Matrix b, int angleStateIdx) { Matrix ret = a.plus(b); - ret.set(angleStateIdx, 0, normalizeAngle(ret.get(angleStateIdx, 0))); + ret.set(angleStateIdx, 0, MathUtil.angleModulus(ret.get(angleStateIdx, 0))); return ret; } @@ -68,16 +69,6 @@ public final class AngleStatistics { return (a, b) -> angleAdd(a, b, angleStateIdx); } - static double normalizeAngle(double angle) { - final double tau = 2 * Math.PI; - - angle -= Math.floor(angle / tau) * tau; - if (angle > Math.PI) { - angle -= tau; - } - return angle; - } - /** * Computes the mean of sigmas with the weights Wm while computing a special angle mean for a * select row. diff --git a/wpimath/src/main/java/edu/wpi/first/wpiutil/math/MathUtil.java b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/MathUtil.java index b04b009f1c..c57ca98422 100644 --- a/wpimath/src/main/java/edu/wpi/first/wpiutil/math/MathUtil.java +++ b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/MathUtil.java @@ -32,22 +32,32 @@ public final class MathUtil { } /** - * Constrains theta to within the range (-pi, pi]. + * Returns modulus of input. * - * @param theta The angle to normalize. - * @return The normalized angle. + * @param input Input value to wrap. + * @param minimumInput The minimum value expected from the input. + * @param maximumInput The maximum value expected from the input. */ - @SuppressWarnings("LocalVariableName") - public static double normalizeAngle(double theta) { - // Constraint theta to within (-3pi, pi) - int nPiPos = (int) ((theta + Math.PI) / 2.0 / Math.PI); - theta -= nPiPos * 2.0 * Math.PI; + public static double inputModulus(double input, double minimumInput, double maximumInput) { + double modulus = maximumInput - minimumInput; - // Cut off the bottom half of the above range to constrain within - // (-pi, pi] - int nPiNeg = (int) ((theta - Math.PI) / 2.0 / Math.PI); - theta -= nPiNeg * 2.0 * Math.PI; + // Wrap input if it's above the maximum input + int numMax = (int) ((input - minimumInput) / modulus); + input -= numMax * modulus; - return theta; + // Wrap input if it's below the minimum input + int numMin = (int) ((input - maximumInput) / modulus); + input -= numMin * modulus; + + return input; + } + + /** + * Wraps an angle to the range -pi to pi radians. + * + * @param angleRadians Angle to wrap in radians. + */ + public static double angleModulus(double angleRadians) { + return inputModulus(angleRadians, -Math.PI, Math.PI); } } diff --git a/wpimath/src/main/native/include/frc/MathUtil.h b/wpimath/src/main/native/include/frc/MathUtil.h new file mode 100644 index 0000000000..274f6d5f07 --- /dev/null +++ b/wpimath/src/main/native/include/frc/MathUtil.h @@ -0,0 +1,45 @@ +// 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. + +#pragma once + +#include + +#include "units/angle.h" + +namespace frc { + +/** + * Returns modulus of input. + * + * @param input Input value to wrap. + * @param minimumInput The minimum value expected from the input. + * @param maximumInput The maximum value expected from the input. + */ +template +constexpr T InputModulus(T input, T minimumInput, T maximumInput) { + T modulus = maximumInput - minimumInput; + + // Wrap input if it's above the maximum input + int numMax = (input - minimumInput) / modulus; + input -= numMax * modulus; + + // Wrap input if it's below the minimum input + int numMin = (input - maximumInput) / modulus; + input -= numMin * modulus; + + return input; +} + +/** + * Wraps an angle to the range -pi to pi radians (-180 to 180 degrees). + * + * @param angle Angle to wrap. + */ +constexpr units::radian_t AngleModulus(units::radian_t angle) { + return InputModulus(angle, units::radian_t{-wpi::math::pi}, + units::radian_t{wpi::math::pi}); +} + +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h b/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h index ddc0c0c690..9e2ed8a956 100644 --- a/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h +++ b/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h @@ -7,17 +7,9 @@ #include #include "Eigen/Core" +#include "frc/MathUtil.h" namespace frc { -inline double NormalizeAngle(double angle) { - static constexpr double tau = 2 * wpi::math::pi; - - angle -= std::floor(angle / tau) * tau; - if (angle > wpi::math::pi) { - angle -= tau; - } - return angle; -} /** * Subtracts a and b while normalizing the resulting value in the selected row @@ -32,7 +24,8 @@ Eigen::Matrix AngleResidual( const Eigen::Matrix& a, const Eigen::Matrix& b, int angleStateIdx) { Eigen::Matrix ret = a - b; - ret[angleStateIdx] = NormalizeAngle(ret[angleStateIdx]); + ret[angleStateIdx] = + AngleModulus(units::radian_t{ret[angleStateIdx]}).to(); return ret; } @@ -65,7 +58,8 @@ Eigen::Matrix AngleAdd( const Eigen::Matrix& a, const Eigen::Matrix& b, int angleStateIdx) { Eigen::Matrix ret = a + b; - ret[angleStateIdx] = NormalizeAngle(ret[angleStateIdx]); + ret[angleStateIdx] = + InputModulus(ret[angleStateIdx], -wpi::math::pi, wpi::math::pi); return ret; } @@ -122,4 +116,5 @@ AngleMean(int angleStateIdx) { return AngleMean(sigmas, Wm, angleStateIdx); }; } + } // namespace frc diff --git a/wpimath/src/main/native/include/units/math.h b/wpimath/src/main/native/include/units/math.h index 43b212e89e..cba14c44f0 100644 --- a/wpimath/src/main/native/include/units/math.h +++ b/wpimath/src/main/native/include/units/math.h @@ -751,24 +751,4 @@ auto fma(const UnitTypeLhs x, const UnitMultiply y, const UnitAdd z) noexcept "Unit types are not compatible."); return resultType(std::fma(x(), y(), resultType(z)())); } - -/** - * Constrains theta to within the range (-pi, pi]. - * - * @param theta Angle to normalize. - */ -constexpr units::radian_t NormalizeAngle(units::radian_t theta) { - units::radian_t pi(wpi::math::pi); - - // Constrain theta to within (-3pi, pi) - const int n_pi_pos = (theta + pi) / 2.0 / pi; - theta = theta - units::radian_t{n_pi_pos * 2.0 * wpi::math::pi}; - - // Cut off the bottom half of the above range to constrain within - // (-pi, pi] - const int n_pi_neg = (theta - pi) / 2.0 / pi; - theta = theta - units::radian_t{n_pi_neg * 2.0 * wpi::math::pi}; - - return theta; -} } // namespace units::math diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/AngleStatisticsTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/AngleStatisticsTest.java index f3003e9731..0503ac865d 100644 --- a/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/AngleStatisticsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/AngleStatisticsTest.java @@ -4,7 +4,6 @@ package edu.wpi.first.wpilibj.estimator; -import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertTrue; import edu.wpi.first.wpiutil.math.Matrix; @@ -42,11 +41,4 @@ public class AngleStatisticsTest { var second = VecBuilder.fill(1, Math.toRadians(359), 1); assertTrue(AngleStatistics.angleAdd(first, second, 1).isEqual(VecBuilder.fill(2, 0, 3), 1e-6)); } - - @Test - public void testNormalize() { - assertEquals(AngleStatistics.normalizeAngle(Math.toRadians(-2000)), Math.toRadians(160), 1e-6); - assertEquals(AngleStatistics.normalizeAngle(Math.toRadians(358)), Math.toRadians(-2), 1e-6); - assertEquals(AngleStatistics.normalizeAngle(Math.toRadians(360)), 0, 1e-6); - } } diff --git a/wpimath/src/test/java/edu/wpi/first/wpiutil/math/MathUtilTest.java b/wpimath/src/test/java/edu/wpi/first/wpiutil/math/MathUtilTest.java index 3b28f9e22d..6808b45fbb 100644 --- a/wpimath/src/test/java/edu/wpi/first/wpiutil/math/MathUtilTest.java +++ b/wpimath/src/test/java/edu/wpi/first/wpiutil/math/MathUtilTest.java @@ -10,10 +10,43 @@ import org.junit.jupiter.api.Test; class MathUtilTest { @Test - void testAngleNormalize() { - assertEquals(MathUtil.normalizeAngle(5 * Math.PI), Math.PI); - assertEquals(MathUtil.normalizeAngle(-5 * Math.PI), Math.PI); - assertEquals(MathUtil.normalizeAngle(Math.PI / 2), Math.PI / 2); - assertEquals(MathUtil.normalizeAngle(-Math.PI / 2), -Math.PI / 2); + void testInputModulus() { + // These tests check error wrapping. That is, the result of wrapping the + // result of an angle reference minus the measurement. + + // Test symmetric range + assertEquals(-20.0, MathUtil.inputModulus(170.0 - (-170.0), -180.0, 180.0)); + assertEquals(-20.0, MathUtil.inputModulus(170.0 + 360.0 - (-170.0), -180.0, 180.0)); + assertEquals(-20.0, MathUtil.inputModulus(170.0 - (-170.0 + 360.0), -180.0, 180.0)); + assertEquals(20.0, MathUtil.inputModulus(-170.0 - 170.0, -180.0, 180.0)); + assertEquals(20.0, MathUtil.inputModulus(-170.0 + 360.0 - 170.0, -180.0, 180.0)); + assertEquals(20.0, MathUtil.inputModulus(-170.0 - (170.0 + 360.0), -180.0, 180.0)); + + // Test range start at zero + assertEquals(340.0, MathUtil.inputModulus(170.0 - 190.0, 0.0, 360.0)); + assertEquals(340.0, MathUtil.inputModulus(170.0 + 360.0 - 190.0, 0.0, 360.0)); + assertEquals(340.0, MathUtil.inputModulus(170.0 - (190.0 + 360), 0.0, 360.0)); + + // Test asymmetric range that doesn't start at zero + assertEquals(-20.0, MathUtil.inputModulus(170.0 - (-170.0), -170.0, 190.0)); + + // Test range with both positive endpoints + assertEquals(2.0, MathUtil.inputModulus(0.0, 1.0, 3.0)); + assertEquals(3.0, MathUtil.inputModulus(1.0, 1.0, 3.0)); + assertEquals(2.0, MathUtil.inputModulus(2.0, 1.0, 3.0)); + assertEquals(3.0, MathUtil.inputModulus(3.0, 1.0, 3.0)); + assertEquals(2.0, MathUtil.inputModulus(4.0, 1.0, 3.0)); + } + + @Test + void testAngleModulus() { + assertEquals(MathUtil.angleModulus(Math.toRadians(-2000)), Math.toRadians(160), 1e-6); + assertEquals(MathUtil.angleModulus(Math.toRadians(358)), Math.toRadians(-2), 1e-6); + assertEquals(MathUtil.angleModulus(Math.toRadians(360)), 0, 1e-6); + + assertEquals(MathUtil.angleModulus(5 * Math.PI), Math.PI); + assertEquals(MathUtil.angleModulus(-5 * Math.PI), Math.PI); + assertEquals(MathUtil.angleModulus(Math.PI / 2), Math.PI / 2); + assertEquals(MathUtil.angleModulus(-Math.PI / 2), -Math.PI / 2); } } diff --git a/wpimath/src/test/native/cpp/MathUtilTest.cpp b/wpimath/src/test/native/cpp/MathUtilTest.cpp new file mode 100644 index 0000000000..ee4fc6dc86 --- /dev/null +++ b/wpimath/src/test/native/cpp/MathUtilTest.cpp @@ -0,0 +1,69 @@ +// 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 "gtest/gtest.h" +#include "units/angle.h" + +#define EXPECT_UNITS_EQ(a, b) \ + EXPECT_FLOAT_EQ((a).to(), (b).to()) + +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)); + + // 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)); + + // Test asymmetric range that doesn't start at zero + EXPECT_FLOAT_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)); + + // Test all supported types + EXPECT_FLOAT_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( + frc::AngleModulus(units::radian_t{-2000 * wpi::math::pi / 180}), + units::radian_t{160 * wpi::math::pi / 180}); + EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t{358 * wpi::math::pi / 180}), + units::radian_t{-2 * wpi::math::pi / 180}); + EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t{2.0 * wpi::math::pi}), + 0_rad); + + EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t(5 * wpi::math::pi)), + units::radian_t(wpi::math::pi)); + EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t(-5 * wpi::math::pi)), + units::radian_t(wpi::math::pi)); + EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t(wpi::math::pi / 2)), + units::radian_t(wpi::math::pi / 2)); + EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t(-wpi::math::pi / 2)), + units::radian_t(-wpi::math::pi / 2)); +} diff --git a/wpimath/src/test/native/cpp/UnitsTest.cpp b/wpimath/src/test/native/cpp/UnitsTest.cpp index 5f27dc6b45..6e9e7119f2 100644 --- a/wpimath/src/test/native/cpp/UnitsTest.cpp +++ b/wpimath/src/test/native/cpp/UnitsTest.cpp @@ -2976,17 +2976,6 @@ TEST_F(UnitMath, abs) { EXPECT_EQ(meter_t(10.0), abs(meter_t(10.0))); } -TEST_F(UnitMath, normalize) { - EXPECT_EQ(NormalizeAngle(radian_t(5 * wpi::math::pi)), - radian_t(wpi::math::pi)); - EXPECT_EQ(NormalizeAngle(radian_t(-5 * wpi::math::pi)), - radian_t(wpi::math::pi)); - EXPECT_EQ(NormalizeAngle(radian_t(wpi::math::pi / 2)), - radian_t(wpi::math::pi / 2)); - EXPECT_EQ(NormalizeAngle(radian_t(-wpi::math::pi / 2)), - radian_t(-wpi::math::pi / 2)); -} - // Constexpr #if !defined(_MSC_VER) || _MSC_VER > 1800 TEST_F(Constexpr, construction) { diff --git a/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp b/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp index 95d8625d18..bbbc398414 100644 --- a/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp @@ -4,6 +4,8 @@ #include +#include + #include "Eigen/Core" #include "frc/estimator/AngleStatistics.h" @@ -33,11 +35,3 @@ TEST(AngleStatisticsTest, TestAdd) { EXPECT_TRUE(frc::AngleAdd<3>(a, b, 1).isApprox(Eigen::Vector3d(2, 0, 3))); } - -TEST(AngleStatisticsTest, TestNormalize) { - EXPECT_NEAR(frc::NormalizeAngle(-2000 * wpi::math::pi / 180), - 160 * wpi::math::pi / 180, 1e-6); - EXPECT_NEAR(frc::NormalizeAngle(358 * wpi::math::pi / 180), - -2 * wpi::math::pi / 180, 1e-6); - EXPECT_NEAR(frc::NormalizeAngle(360 * wpi::math::pi / 180), 0, 1e-6); -}