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

8
wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/AngleStatisticsTest.java
View File

@@ -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);
}
}

43
wpimath/src/test/java/edu/wpi/first/wpiutil/math/MathUtilTest.java
View File

@@ -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);
}
}