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

[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

13
wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/RamseteControllerTest.java
View File

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