From 3f498897ec790d6124c3a7b8eaf5b99b8aa18e38 Mon Sep 17 00:00:00 2001 From: iefomit Date: Sun, 29 Mar 2026 20:03:33 -0700 Subject: [PATCH] wrap gyro --- .../java/frc/robot/subsystems/drivetrain/Drivetrain.java | 7 +++++++ 1 file changed, 7 insertions(+) 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)); -- 2.39.5