Team2890/YAGSL
4
0
Fork 0
mirror of https://github.com/BroncBotz3481/YAGSL synced 2026-06-19 06:21:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

Update to swervelib 2024.5.0.0

Browse Source
This commit is contained in:
thenetworkgrinch
2024-07-29 15:14:25 -05:00
parent a64d1d3215
commit 89e4163951
120 changed files with 1420 additions and 439 deletions
Download Patch File Download Diff File Expand all files Collapse all files

36
swervelib/math/SwerveMath.java
View File

@@ -359,6 +359,9 @@ public class SwerveMath
/**
* Put an angle within the 360 deg scope of a reference. For example, given a scope reference of 756 degrees, assumes
* the full scope is (720-1080), and places an angle of 22 degrees into it, returning 742 deg.
*
* A more formal definition: returns the closest angle {@code n} to {@code scopeReference} such that {@code n} is
* congruent to {@code newAngle}.
*
* @param scopeReference Current Angle (deg)
* @param newAngle Target Angle (deg)
@@ -366,34 +369,11 @@ public class SwerveMath
*/
public static double placeInAppropriate0To360Scope(double scopeReference, double newAngle)
{
double lowerBound;
double upperBound;
double lowerOffset = scopeReference % 360;
if (lowerOffset >= 0)
{
lowerBound = scopeReference - lowerOffset;
upperBound = scopeReference + (360 - lowerOffset);
} else
{
upperBound = scopeReference - lowerOffset;
lowerBound = scopeReference - (360 + lowerOffset);
}
while (newAngle < lowerBound)
{
newAngle += 360;
}
while (newAngle > upperBound)
{
newAngle -= 360;
}
if (newAngle - scopeReference > 180)
{
newAngle -= 360;
} else if (newAngle - scopeReference < -180)
{
newAngle += 360;
}
return newAngle;
// Figure out how many revolutions from the angle to the reference
double diffRevs = Math.round((scopeReference - newAngle) / 360) * 360;
// Add that many revolutions
return diffRevs + newAngle;
}
/**