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 989ec9af2e49b9bfa83433c7098f7cd050391098..81efc82a3a7955db6bb5e7b9726e8c0a9b34b1d8 100644 (file)
--- 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 bcd263b878433b4935e6a74c73ffd54f96833ee1..7e8d3f23219f252c4192d2d8d164afe9a26c3d41 100644 (file)
--- 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);
     }
 
     /**