From: iefomit <timofei.stem@gmail.com>
Date: Tue, 31 Mar 2026 17:34:00 +0000 (-0700)
Subject: angle mod, comments
X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=1ed3453c2a4532fe99ed8197a825fdcca629ff95;p=FRC2026.git

angle mod, comments
---

diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
index 989ec9a..81efc82 100644
--- a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
@@ -122,10 +122,10 @@ public class Drivetrain extends SubsystemBase {
     
     // gyro history for timestamp interpolation
     private static final int GYRO_HISTORY_SIZE = 50;
-    private double[] gyroTimestampsHistory = new double[GYRO_HISTORY_SIZE];
-    private double[] gyroYawsHistory = new double[GYRO_HISTORY_SIZE];
-    private int gyroHistoryHead = 0;
-    private int gyroHistoryCount = 0;
+    private double[] gyroTimestampsHistory = new double[GYRO_HISTORY_SIZE]; // circular buffer of gyro timestamps for interpolation
+    private double[] gyroYawsHistory = new double[GYRO_HISTORY_SIZE]; // circular buffer of gyro yaw angles corresponding to timestamps
+    private int gyroHistoryHead = 0; // index of the most recent entry in the circular buffer
+    private int gyroHistoryCount = 0; // number of valid entries in the history buffer
 
     private final Field2d field = new Field2d();
 
diff --git a/src/main/java/frc/robot/util/Vision/GyroBiasEstimator.java b/src/main/java/frc/robot/util/Vision/GyroBiasEstimator.java
index bcd263b..7e8d3f2 100644
--- a/src/main/java/frc/robot/util/Vision/GyroBiasEstimator.java
+++ b/src/main/java/frc/robot/util/Vision/GyroBiasEstimator.java
@@ -1,5 +1,6 @@
 package frc.robot.util.Vision;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Pose3d;
 import frc.robot.constants.GyroBiasConstants;
 
@@ -140,13 +141,7 @@ public class GyroBiasEstimator {
      * normalize angle to [-PI, PI]
      */
     private double normalizeAngle(double angle) {
-        while (angle > Math.PI) {
-            angle -= 2 * Math.PI;
-        }
-        while (angle < -Math.PI) {
-            angle += 2 * Math.PI;
-        }
-        return angle;
+        return MathUtil.angleModulus(angle);
     }
 
     /**