From: iefomit <timofei.stem@gmail.com>
Date: Wed, 1 Apr 2026 06:46:33 +0000 (-0700)
Subject: more fixes
X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=7aa577dbe8c50ee18e08f59403347e944d6de815;p=FRC2026.git

more fixes
---

diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java
index a7ff0b3..d2303c5 100644
--- a/src/main/java/frc/robot/constants/VisionConstants.java
+++ b/src/main/java/frc/robot/constants/VisionConstants.java
@@ -67,8 +67,8 @@ public class VisionConstants {
          */
         public static final double MAX_DISTANCE = 6;
 
-        /** If vision should use manual calculations */
-        public static final boolean USE_MANUAL_CALCULATIONS = false; // changed from true for gyro bias correction feature
+        /** If vision should use manual calculations (yawFunction-based vs referencePose-based). Changed to false to support gyro bias correction. */
+        public static final boolean USE_MANUAL_CALCULATIONS = false;
 
         // <ol start="0"> did not work
         /**
diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
index 2c4f607..48ea33e 100644
--- a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
@@ -326,22 +326,17 @@ public class Drivetrain extends SubsystemBase {
         }
 
         if (VisionConstants.ENABLED) {
+            recordGyroReading(gyroInputs.yawPosition.getRadians());
+
             if (vision != null && visionEnabled && visionEnableTimer.hasElapsed(5)) {
-                vision.updateOdometry(poseEstimator, time -> getPoseAt(time).getRotation().getRadians(), slipped);
+                ArrayList<EstimatedRobotPose> visionPoses = vision.updateOdometry(poseEstimator, time -> getPoseAt(time).getRotation().getRadians(), slipped);
 
                 if (vision.canSeeTag()) {
                     slipped = false;
                     modulePoses.reset();
 
-                    // record reading for timestamp interpolation
-                    recordGyroReading(gyroInputs.yawPosition.getRadians());
-
                     double currentGyroYaw = gyroInputs.yawPosition.getRadians();
 
-                    // calculate bias between vision-derived and gyro yaw
-                    // this compensates for gyro drift over time
-                    ArrayList<EstimatedRobotPose> visionPoses = vision.getEstimatedPoses(getPose());
-
                     for (EstimatedRobotPose visionPose : visionPoses) {
                         if (visionPose.estimatedPose != null && visionPose.timestampSeconds > 0) {
                             // get yaw from the vision pose estimate
diff --git a/src/main/java/frc/robot/util/Vision/GyroBiasEstimator.java b/src/main/java/frc/robot/util/Vision/GyroBiasEstimator.java
index 4c871bb..a98b7b4 100644
--- a/src/main/java/frc/robot/util/Vision/GyroBiasEstimator.java
+++ b/src/main/java/frc/robot/util/Vision/GyroBiasEstimator.java
@@ -103,13 +103,8 @@ public class GyroBiasEstimator {
             return 0.0;
         }
 
-        // use weighted average if we have enough weight, otherwise use EMA
-        double avgBias;
-        if (totalWeight >= GyroBiasConstants.MIN_TOTAL_WEIGHT) {
-            avgBias = weightedBiasSum / totalWeight;
-        } else {
-            avgBias = emaBias;
-        }
+        // use weighted average
+        double avgBias = weightedBiasSum / totalWeight;
 
         // reset
         weightedBiasSum = 0.0;
diff --git a/src/main/java/frc/robot/util/Vision/Vision.java b/src/main/java/frc/robot/util/Vision/Vision.java
index d92b3de..0bafa21 100644
--- a/src/main/java/frc/robot/util/Vision/Vision.java
+++ b/src/main/java/frc/robot/util/Vision/Vision.java
@@ -336,8 +336,9 @@ public class Vision {
    * @param poseEstimator The pose estimator to update
    * @param yawFunction A function that returns the yaw as a double given the timestamp
    * @param slipped True if the wheels have slipped, false otherwise
+   * @return The list of estimated robot poses from vision
    */
-  public void updateOdometry(SwerveDrivePoseEstimator poseEstimator, DoubleUnaryOperator yawFunction, boolean slipped){
+  public ArrayList<EstimatedRobotPose> updateOdometry(SwerveDrivePoseEstimator poseEstimator, DoubleUnaryOperator yawFunction, boolean slipped){
     // Simulate vision
     // 2 ifs to avoid warning
     if(VisionConstants.ENABLED_SIM){
@@ -363,6 +364,7 @@ public class Vision {
       );
       sawTag = true;
     }
+    return estimatedPoses;
   }
 
   /**