Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-21 01:01:43 +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

69
wpimath/src/test/native/cpp/MathUtilTest.cpp Normal file
View File

@@ -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<double>(), (b).to<double>())
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<double>(170.0 - (-170.0), -170.0, 190.0));
EXPECT_EQ(-20, frc::InputModulus<int>(170 - (-170), -170, 190));
EXPECT_EQ(-20_deg, frc::InputModulus<units::degree_t>(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));
}

11
wpimath/src/test/native/cpp/UnitsTest.cpp
View File

@@ -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) {

10
wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp
View File

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