diff --git a/wpilibc/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp b/wpilibc/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp new file mode 100644 index 0000000000..fb72a4aec2 --- /dev/null +++ b/wpilibc/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp @@ -0,0 +1,51 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include +#include +#include + +#include "frc/controller/HolonomicDriveController.h" +#include "frc/trajectory/TrajectoryGenerator.h" +#include "gtest/gtest.h" + +#define EXPECT_NEAR_UNITS(val1, val2, eps) \ + EXPECT_LE(units::math::abs(val1 - val2), eps) + +static constexpr units::meter_t kTolerance{1 / 12.0}; +static constexpr units::radian_t kAngularTolerance{2.0 * wpi::math::pi / 180.0}; + +TEST(HolonomicDriveControllerTest, ReachesReference) { + frc::HolonomicDriveController controller{ + frc2::PIDController{1.0, 0.0, 0.0}, frc2::PIDController{1.0, 0.0, 0.0}, + frc::ProfiledPIDController{ + 1.0, 0.0, 0.0, + frc::TrapezoidProfile::Constraints{ + 6.28_rad_per_s, 3.14_rad_per_s / 1_s}}}; + + frc::Pose2d robotPose{2.7_m, 23_m, frc::Rotation2d{0_deg}}; + + auto waypoints = std::vector{frc::Pose2d{2.75_m, 22.521_m, 0_rad}, + frc::Pose2d{24.73_m, 19.68_m, 5.846_rad}}; + auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory( + waypoints, {8.0_mps, 4.0_mps_sq}); + + constexpr auto kDt = 0.02_s; + auto totalTime = trajectory.TotalTime(); + for (size_t i = 0; i < (totalTime / kDt).to(); ++i) { + auto state = trajectory.Sample(kDt * i); + auto [vx, vy, omega] = controller.Calculate(robotPose, state, 0_rad); + + robotPose = robotPose.Exp(frc::Twist2d{vx * kDt, vy * kDt, omega * kDt}); + } + + 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); +} diff --git a/wpilibc/src/test/native/cpp/controller/RamseteControllerTest.cpp b/wpilibc/src/test/native/cpp/controller/RamseteControllerTest.cpp index f7e9f2c754..fb343856f0 100644 --- a/wpilibc/src/test/native/cpp/controller/RamseteControllerTest.cpp +++ b/wpilibc/src/test/native/cpp/controller/RamseteControllerTest.cpp @@ -6,7 +6,6 @@ /*----------------------------------------------------------------------------*/ #include -#include #include "frc/controller/RamseteController.h" #include "frc/trajectory/TrajectoryGenerator.h" @@ -18,16 +17,6 @@ static constexpr units::meter_t kTolerance{1 / 12.0}; static constexpr units::radian_t kAngularTolerance{2.0 * wpi::math::pi / 180.0}; -units::radian_t boundRadians(units::radian_t value) { - while (value > units::radian_t{wpi::math::pi}) { - value -= units::radian_t{wpi::math::pi * 2}; - } - while (value <= units::radian_t{-wpi::math::pi}) { - value += units::radian_t{wpi::math::pi * 2}; - } - return value; -} - TEST(RamseteControllerTest, ReachesReference) { frc::RamseteController controller{2.0, 0.7}; frc::Pose2d robotPose{2.7_m, 23_m, frc::Rotation2d{0_deg}}; @@ -50,7 +39,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(boundRadians(endPose.Rotation().Radians() - - robotPose.Rotation().Radians()), + EXPECT_NEAR_UNITS(units::math::NormalizeAngle(endPose.Rotation().Radians() - + robotPose.Rotation().Radians()), 0_rad, 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 1b41479122..a2b6e22e81 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 @@ -16,6 +16,7 @@ import edu.wpi.first.wpilibj.geometry.Rotation2d; import edu.wpi.first.wpilibj.geometry.Twist2d; import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig; import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator; +import edu.wpi.first.wpiutil.math.MathUtil; import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertEquals; @@ -24,16 +25,6 @@ class RamseteControllerTest { private static final double kTolerance = 1 / 12.0; private static final double kAngularTolerance = Math.toRadians(2); - private static double boundRadians(double value) { - while (value > Math.PI) { - value -= Math.PI * 2; - } - while (value <= -Math.PI) { - value += Math.PI * 2; - } - return value; - } - @Test @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops") void testReachesReference() { @@ -68,7 +59,7 @@ class RamseteControllerTest { () -> assertEquals(endPose.getY(), finalRobotPose.getY(), kTolerance), () -> assertEquals(0.0, - boundRadians(endPose.getRotation().getRadians() + MathUtil.normalizeAngle(endPose.getRotation().getRadians() - finalRobotPose.getRotation().getRadians()), kAngularTolerance) ); diff --git a/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/MathUtil.java b/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/MathUtil.java index bd555dc770..bd03f6b335 100644 --- a/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/MathUtil.java +++ b/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/MathUtil.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -33,4 +33,24 @@ public final class MathUtil { public static double clamp(double value, double low, double high) { return Math.max(low, Math.min(value, high)); } + + /** + * Constrains theta to within the range (-pi, pi]. + * + * @param theta The angle to normalize. + * @return The normalized angle. + */ + @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; + + // 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; + + return theta; + } } diff --git a/wpiutil/src/main/native/include/units/math.h b/wpiutil/src/main/native/include/units/math.h index 5736bcf415..ccb3a62698 100644 --- a/wpiutil/src/main/native/include/units/math.h +++ b/wpiutil/src/main/native/include/units/math.h @@ -31,6 +31,8 @@ #include +#include + #include "units/angle.h" #include "units/base.h" #include "units/dimensionless.h" @@ -754,5 +756,24 @@ auto fma(const UnitTypeLhs x, const UnitMultiply y, const UnitAdd z) noexcept 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 math } // namespace units diff --git a/wpiutil/src/test/java/edu/wpi/first/wpiutil/math/MathUtilTest.java b/wpiutil/src/test/java/edu/wpi/first/wpiutil/math/MathUtilTest.java new file mode 100644 index 0000000000..14a0f7c256 --- /dev/null +++ b/wpiutil/src/test/java/edu/wpi/first/wpiutil/math/MathUtilTest.java @@ -0,0 +1,22 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpiutil.math; + +import org.junit.jupiter.api.Test; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +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); + } +} diff --git a/wpiutil/src/test/native/cpp/UnitsTest.cpp b/wpiutil/src/test/native/cpp/UnitsTest.cpp index f427e1ecc0..1bfd22029d 100644 --- a/wpiutil/src/test/native/cpp/UnitsTest.cpp +++ b/wpiutil/src/test/native/cpp/UnitsTest.cpp @@ -2979,11 +2979,15 @@ TEST_F(UnitMath, abs) { EXPECT_EQ(meter_t(10.0), abs(meter_t(10.0))); } -TEST_F(UnitMath, fma) { - meter_t x(2.0); - meter_t y(3.0); - square_meter_t z(1.0); - EXPECT_EQ(square_meter_t(7.0), fma(x, y, z)); +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