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

22
wpiutil/src/main/java/edu/wpi/first/wpiutil/math/MathUtil.java
View File

@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -33,4 +33,24 @@ public final class MathUtil {
public static double clamp(double value, double low, double high) {
return Math.max(low, Math.min(value, high));
}
/**
* Constrains theta to within the range (-pi, pi].
*
* @param theta The angle to normalize.
* @return The normalized angle.
*/
@SuppressWarnings("LocalVariableName")
public static double normalizeAngle(double theta) {
// Constraint theta to within (-3pi, pi)
int nPiPos = (int) ((theta + Math.PI) / 2.0 / Math.PI);
theta -= nPiPos * 2.0 * Math.PI;
// Cut off the bottom half of the above range to constrain within
// (-pi, pi]
int nPiNeg = (int) ((theta - Math.PI) / 2.0 / Math.PI);
theta -= nPiNeg * 2.0 * Math.PI;
return theta;
}
}