more fixes

diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java
index a7ff0b307f24ebd2d455b2367e7a9bbca4955d07..d2303c592b265be0a05ab603600981c0641dee0b 100644 (file)
--- 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 2c4f6074d4f301c2cfa16f31a8cadc8332b0bbe6..48ea33e690e3d41a109e18fac4f7d433480a00f0 100644 (file)
--- 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 4c871bba844705868eaa68377697aa7547e53beb..a98b7b413b1722c08bad6c8e715f7b461bd4ee27 100644 (file)
--- 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 d92b3de0e3b56ff5a216e0352b018aa5fae1d431..0bafa21e1cf2ff2def5dd4ba2cd4b85db0174483 100644 (file)
--- 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;
   }
 
   /**