wrap gyro

diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
index 9befad4df3fa980defdf300f7e90daf5f20ef381..989ec9af2e49b9bfa83433c7098f7cd050391098 100644 (file)
--- a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
@@ -339,6 +339,13 @@ public class Drivetrain extends SubsystemBase {
                             double gyroYawAtTimestamp = getGyroYawAtTimestamp(visionPose.timestampSeconds);
 
                             if (!Double.isNaN(gyroYawAtTimestamp)) {
+                                // wrap gyro yaw
+                                while (gyroYawAtTimestamp > Math.PI) {
+                                    gyroYawAtTimestamp -= 2 * Math.PI;
+                                }
+                                while (gyroYawAtTimestamp < -Math.PI) {
+                                    gyroYawAtTimestamp += 2 * Math.PI;
+                                }
 
                                 Logger.recordOutput("GyroYaw", Math.toDegrees(gyroYawAtTimestamp));
                                 Logger.recordOutput("VisionYaw", Math.toDegrees(visionYaw));