From 1ed3453c2a4532fe99ed8197a825fdcca629ff95 Mon Sep 17 00:00:00 2001 From: iefomit Date: Tue, 31 Mar 2026 10:34:00 -0700 Subject: [PATCH] angle mod, comments --- .../java/frc/robot/subsystems/drivetrain/Drivetrain.java | 8 ++++---- .../java/frc/robot/util/Vision/GyroBiasEstimator.java | 9 ++------- 2 files changed, 6 insertions(+), 11 deletions(-) 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); } /** -- 2.39.5