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

[wpiutil] Add angle normalization method

Browse Source
This commit is contained in:
Prateek Machiraju
2020-07-13 15:53:16 -04:00
committed by Peter Johnson
parent 399684a58f
commit af588adce5
7 changed files with 128 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
wpilibc/src/test/native/cpp/controller/RamseteControllerTest.cpp
View File

@@ -6,7 +6,6 @@
/*----------------------------------------------------------------------------*/
#include <units/math.h>
#include <wpi/math>
#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);
}