Team2890/YAGSL
4
0
Fork 0
mirror of https://github.com/BroncBotz3481/YAGSL synced 2026-07-05 07:31:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

Fixed issue with angle optimization

Browse Source
This commit is contained in:
thenetworkgrinch
2023-08-29 19:59:07 -05:00
parent 38c7bdba7d
commit 12f6e0ed38
138 changed files with 8913 additions and 1020 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
swervelib/math/SwerveModuleState2.java
View File

@@ -65,11 +65,11 @@ public class SwerveModuleState2 extends SwerveModuleState
SwerveModuleState2 optimized = new SwerveModuleState2(SwerveModuleState.optimize(desiredState, currentAngle),
desiredState.omegaRadPerSecond);
if (desiredState.angle.equals(currentAngle) || desiredState.angle.equals(
optimized.angle.rotateBy(Rotation2d.fromDegrees(180))) || moduleSteerFeedForwardClosedLoop == 0)
optimized.angle.rotateBy(Rotation2d.fromDegrees(90))) || moduleSteerFeedForwardClosedLoop == 0)
{
optimized.omegaRadPerSecond = 0;
}
if (desiredState.angle.equals(optimized.angle.rotateBy(Rotation2d.fromDegrees(180))))
if (desiredState.angle.equals(optimized.angle.rotateBy(Rotation2d.fromDegrees(90))))
{
desiredState.omegaRadPerSecond = 0;
return desiredState;