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] Deduplicate angle modulus functions (#2998)

Browse Source 
frc::NormalizeAngle(), units::math::NormalizeAngle(), and
frc::GetModulusError() were replaced with frc::InputModulus() and
frc::AngleModulus().

They were placed in wpimath/src/main/native/include/frc/MathUtil.h for
C++ and wpimath/src/main/java/edu/wpi/first/wpiutil/math/MathUtil.java
for Java.
This commit is contained in:
Tyler Veness
2021-01-01 16:22:00 -08:00
committed by GitHub
parent bf8c0da4be
commit 62f0f8190d
23 changed files with 210 additions and 219 deletions
Download Patch File Download Diff File Expand all files Collapse all files

39
wpilibc/src/test/native/cpp/controller/ControllerUtilTest.cpp
View File

@@ -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 <units/angle.h>
#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<double>(170.0, -170.0, -170.0, 190.0));
EXPECT_EQ(-20, frc::GetModulusError<int>(170, -170, -170, 190));
EXPECT_EQ(-20_deg, frc::GetModulusError<units::degree_t>(170_deg, -170_deg,
-170_deg, 190_deg));
}

5
wpilibc/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp
View File

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

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

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