From: iefomit <timofei.stem@gmail.com>
Date: Mon, 30 Mar 2026 03:03:33 +0000 (-0700)
Subject: wrap gyro
X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=3f498897ec790d6124c3a7b8eaf5b99b8aa18e38;p=FRC2026.git

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 9befad4..989ec9a 100644
--- 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));