]> git.taranathan.com Git - FRC2026.git/commitdiff
wrap gyro
authoriefomit <timofei.stem@gmail.com>
Mon, 30 Mar 2026 03:03:33 +0000 (20:03 -0700)
committeriefomit <timofei.stem@gmail.com>
Mon, 30 Mar 2026 03:03:33 +0000 (20:03 -0700)
src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java

index 9befad4df3fa980defdf300f7e90daf5f20ef381..989ec9af2e49b9bfa83433c7098f7cd050391098 100644 (file)
@@ -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));