From 633aac0918061bb32a62378831c12dfb1ba6c913 Mon Sep 17 00:00:00 2001
From: N/A <cosmicaura3@gmail.com>
Date: Sun, 15 Mar 2026 17:03:43 -0500
Subject: [PATCH 01/26] fix: remove example constant

---
 src/main/java/frc/robot/Constants.java | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 87eacde..ff572af 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -483,7 +483,6 @@ public static class VisionOdometryConstants {
                 Units.inchesToMeters(9.725),
                 Units.inchesToMeters(20.25));
 
-        long num = 999_999_999;
         public static final Vector<N3> LEFT_SINGLE_TAG_STANDARD_DEVIATION = VecBuilder.fill(4, 4, 8);
         public static final Vector<N3> LEFT_MULTI_TAG_STANDARD_DEVIATION = VecBuilder.fill(0.5, 0.5, 1);
 

From a0bf1c1178571ab0aff42714f0e791f022531702 Mon Sep 17 00:00:00 2001
From: N/A <cosmicaura3@gmail.com>
Date: Sun, 15 Mar 2026 17:05:42 -0500
Subject: [PATCH 02/26] feat: improve vision odometry through consensus gating

---
 .../vision/PhotonVisionOdometry.java          | 148 ++++++++++++++++--
 .../robot/util/vision/VisionObservation.java  |  41 +++++
 2 files changed, 179 insertions(+), 10 deletions(-)
 create mode 100644 src/main/java/frc/robot/util/vision/VisionObservation.java

diff --git a/src/main/java/frc/robot/subsystems/vision/PhotonVisionOdometry.java b/src/main/java/frc/robot/subsystems/vision/PhotonVisionOdometry.java
index c5d54da..ad9ed39 100644
--- a/src/main/java/frc/robot/subsystems/vision/PhotonVisionOdometry.java
+++ b/src/main/java/frc/robot/subsystems/vision/PhotonVisionOdometry.java
@@ -2,14 +2,21 @@
 
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Transform2d;
-
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.units.measure.Distance;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.Robot;
 import frc.robot.util.vision.Cameras;
+import frc.robot.util.vision.VisionObservation;
 
 import java.awt.Desktop;
 import java.util.ArrayList;
@@ -18,9 +25,13 @@
 import java.util.function.Supplier;
 import org.photonvision.EstimatedRobotPose;
 import org.photonvision.PhotonUtils;
+import org.photonvision.PhotonPoseEstimator.PoseStrategy;
 import org.photonvision.simulation.VisionSystemSim;
 import org.photonvision.targeting.PhotonPipelineResult;
 import org.photonvision.targeting.PhotonTrackedTarget;
+
+import com.fasterxml.jackson.databind.ser.impl.PropertySerializerMap;
+
 import swervelib.SwerveDrive;
 import swervelib.telemetry.SwerveDriveTelemetry;
 
@@ -102,6 +113,83 @@ public static Pose2d getAprilTagPose(int aprilTag, Transform2d robotOffset) {
 
   }
   
+  /*
+   * POSITION ESTIMATION
+   */
+  private double averageTagDistance(EstimatedRobotPose est) {
+    if (est.targetsUsed == null || est.targetsUsed.isEmpty()) return Double.POSITIVE_INFINITY;
+
+    double sum = 0.0;
+    for (var target : est.targetsUsed) {
+      sum += target.getBestCameraToTarget().getTranslation().getNorm();
+    }
+    return sum / est.targetsUsed.size();
+  }
+
+  private double maxAmbiguity(EstimatedRobotPose est) {
+    if (est.targetsUsed == null || est.targetsUsed.isEmpty()) return 1.0;
+
+    double max = 0.0;
+    for (var target : est.targetsUsed) {
+      max = Math.max(max, target.getPoseAmbiguity());
+    }
+    return max;
+  }
+
+  private boolean agrees(Pose2d posA, Pose2d posB, double maxXY, double maxThetaRad) {
+    return posA.getTranslation().getDistance(posB.getTranslation()) < maxXY
+        && Math.abs(posA.getRotation().minus(posB.getRotation()).getRadians()) < maxThetaRad;
+  }
+
+  private List<VisionObservation> filterByConsensus(List<VisionObservation> obs) {
+    List<VisionObservation> accepted = new ArrayList<>();
+
+    for (VisionObservation obsA : obs) {
+      int agreements = 0;
+      for (VisionObservation obsB : obs) {
+        if (obsA == obsB) continue;
+        if (agrees(obsA.pose, obsB.pose, 0.6, Math.toRadians(15))) {
+          agreements++;
+        }
+      }
+
+      // keep if another camera agrees, or if it is strong multitag
+      if (agreements > 0 || (obsA.isMultiTag && obsA.tagCount >= 2 && obsA.avgTagDistance < 4.0)) {
+        accepted.add(obsA);
+      }
+    }
+
+    return accepted;
+  }
+
+  private boolean shouldAcceptEnabledMeasurement(
+      VisionObservation obs,
+      Pose2d currentPose,
+      ChassisSpeeds speeds,
+      int supportCount) {
+
+    double translationalError =
+        currentPose.getTranslation().getDistance(obs.pose.getTranslation());
+    double headingError =
+        Math.abs(currentPose.getRotation().minus(obs.pose.getRotation()).getRadians());
+
+    double speed = Math.hypot(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond);
+
+    // Base tolerance
+    double maxTranslationJump = 0.5 + 0.35 * speed;
+    double maxHeadingJump = Math.toRadians(20);
+
+    // Strong multitag or multi-camera consensus may correct larger errors
+    boolean strongVision = obs.isMultiTag && obs.avgTagDistance < 4.0;
+    boolean hasSupport = supportCount >= 2;
+
+    if (translationalError < maxTranslationJump && headingError < maxHeadingJump) {
+      return true;
+    }
+
+    return strongVision || hasSupport;
+  }
+
   /**
    * Update the pose estimation inside of {@link SwerveDrive} with all of the
    * given poses.
@@ -122,21 +210,61 @@ public void updatePoseEstimation(SwerveDrive swerveDrive) {
        */
       visionSim.update(swerveDrive.getSimulationDriveTrainPose().get());
     }
+    
+    // Get the robot's current position for reference.
+    Pose2d currentPose = swerveDrive.getPose();
+    ChassisSpeeds speeds = swerveDrive.getRobotVelocity();
+
+    List<VisionObservation> observations = new ArrayList<>();
     for (Cameras camera : Cameras.values()) {
       Optional<EstimatedRobotPose> poseEst = getEstimatedGlobalPose(camera);
-      if (poseEst.isPresent()) {
+      if (!poseEst.isPresent()) continue; 
 
-        var pose = poseEst.get();
-        swerveDrive.addVisionMeasurement(
-            pose.estimatedPose.toPose2d(),
-            pose.timestampSeconds,
-            camera.curStdDevs);
+      EstimatedRobotPose estPos =  poseEst.get();
+      Pose2d estPose2d = estPos.estimatedPose.toPose2d();
+      
+      int tagsUsed = estPos.targetsUsed == null ? 0 : estPos.targetsUsed.size();
+      if (tagsUsed == 0) continue;
 
-        SmartDashboard.putNumber("VisionX", pose.estimatedPose.getX());
-        SmartDashboard.putNumber("VisionY", pose.estimatedPose.getY());
-      }
+      double avgDist = averageTagDistance(estPos);
+      double maxAmbiguity = maxAmbiguity(estPos);
+      boolean isMultiTag = tagsUsed >= 2;
+      Matrix<N3, N1> currStdDevs = camera.curStdDevs;
+
+      observations.add(new VisionObservation(
+        camera,
+        estPos,
+        estPose2d, 
+        maxAmbiguity, 
+        tagsUsed, 
+        avgDist, 
+        maxAmbiguity, 
+        isMultiTag, 
+        currStdDevs
+        ));
+    }
+
+    if (observations.isEmpty()) return;
+
+    // Reject inconsistent outliers using cross-camera agreement
+    List<VisionObservation> filtered = filterByConsensus(observations);
+
+    if (filtered.isEmpty()) {
+      filtered = observations.stream()
+          .filter(o -> o.isMultiTag)
+          .toList();
     }
 
+    if (filtered.isEmpty()) {
+      return;
+    }
+
+    // Update the robot's position using the valid position observations.
+    for (VisionObservation obs : filtered) {
+      if (shouldAcceptEnabledMeasurement(obs, currentPose, speeds, filtered.size())) {
+        swerveDrive.addVisionMeasurement(obs.pose, obs.timestamp, obs.stdDevs);
+      }
+    } 
   }
 
   /**
diff --git a/src/main/java/frc/robot/util/vision/VisionObservation.java b/src/main/java/frc/robot/util/vision/VisionObservation.java
new file mode 100644
index 0000000..bcac2d9
--- /dev/null
+++ b/src/main/java/frc/robot/util/vision/VisionObservation.java
@@ -0,0 +1,41 @@
+package frc.robot.util.vision;
+
+import org.photonvision.EstimatedRobotPose;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+
+public class VisionObservation {
+  public final Cameras camera;
+  public final EstimatedRobotPose estimate;
+  public final Pose2d pose;
+  public final double timestamp;
+  public final int tagCount;
+  public final double avgTagDistance;
+  public final double ambiguity;
+  public final boolean isMultiTag;
+  public final Matrix<N3, N1> stdDevs;
+
+  public VisionObservation(
+      Cameras camera,
+      EstimatedRobotPose estimate,
+      Pose2d pose,
+      double timestamp,
+      int tagCount,
+      double avgTagDistance,
+      double ambiguity,
+      boolean isMultiTag,
+      Matrix<N3, N1> stdDevs) {
+    this.camera = camera;
+    this.estimate = estimate;
+    this.pose = pose;
+    this.timestamp = timestamp;
+    this.tagCount = tagCount;
+    this.avgTagDistance = avgTagDistance;
+    this.ambiguity = ambiguity;
+    this.isMultiTag = isMultiTag;
+    this.stdDevs = stdDevs;
+  }
+}
\ No newline at end of file

From 42df9d160b5e3477d116e4e92c2644b539c9f483 Mon Sep 17 00:00:00 2001
From: N/A <cosmicaura3@gmail.com>
Date: Sun, 15 Mar 2026 17:12:05 -0500
Subject: [PATCH 03/26] docs: add javadoc comments to updated code

---
 .../vision/PhotonVisionOdometry.java          | 394 ++++++++++++------
 1 file changed, 273 insertions(+), 121 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/vision/PhotonVisionOdometry.java b/src/main/java/frc/robot/subsystems/vision/PhotonVisionOdometry.java
index ad9ed39..31eaea2 100644
--- a/src/main/java/frc/robot/subsystems/vision/PhotonVisionOdometry.java
+++ b/src/main/java/frc/robot/subsystems/vision/PhotonVisionOdometry.java
@@ -116,157 +116,309 @@ public static Pose2d getAprilTagPose(int aprilTag, Transform2d robotOffset) {
   /*
    * POSITION ESTIMATION
    */
-  private double averageTagDistance(EstimatedRobotPose est) {
-    if (est.targetsUsed == null || est.targetsUsed.isEmpty()) return Double.POSITIVE_INFINITY;
+/**
+ * Computes the average distance from the camera to each AprilTag used in a pose estimate.
+ *
+ * <p>This is used as a rough quality metric for the estimate. In general, estimates built
+ * from closer tags are more trustworthy than estimates built from distant tags, since distance
+ * tends to amplify vision noise and pose uncertainty.
+ *
+ * @param est the estimated robot pose containing the list of AprilTag targets used
+ * @return the average camera-to-tag distance in meters, or {@link Double#POSITIVE_INFINITY}
+ *     if no targets were used
+ */
+private double averageTagDistance(EstimatedRobotPose est) {
+  // If no tags contributed to this estimate, treat the distance as effectively unusable.
+  if (est.targetsUsed == null || est.targetsUsed.isEmpty()) return Double.POSITIVE_INFINITY;
 
-    double sum = 0.0;
-    for (var target : est.targetsUsed) {
-      sum += target.getBestCameraToTarget().getTranslation().getNorm();
-    }
-    return sum / est.targetsUsed.size();
+  double sum = 0.0;
+
+  /*
+   * Sum the Euclidean distance from the camera to every tag that contributed
+   * to this pose estimate.
+   */
+  for (var target : est.targetsUsed) {
+    sum += target.getBestCameraToTarget().getTranslation().getNorm();
   }
 
-  private double maxAmbiguity(EstimatedRobotPose est) {
-    if (est.targetsUsed == null || est.targetsUsed.isEmpty()) return 1.0;
+  // Return the mean distance across all contributing tags.
+  return sum / est.targetsUsed.size();
+}
 
-    double max = 0.0;
-    for (var target : est.targetsUsed) {
-      max = Math.max(max, target.getPoseAmbiguity());
-    }
-    return max;
-  }
+/**
+ * Finds the highest pose ambiguity among all AprilTags used in a pose estimate.
+ *
+ * <p>Pose ambiguity is a PhotonVision-provided measure describing how uncertain a tag-based
+ * pose solve is. A larger ambiguity generally indicates a less trustworthy estimate.
+ * This method uses the worst contributing tag as a conservative quality metric.
+ *
+ * @param est the estimated robot pose containing the list of AprilTag targets used
+ * @return the maximum ambiguity value across all used tags, or {@code 1.0} if no targets were used
+ */
+private double maxAmbiguity(EstimatedRobotPose est) {
+  // If no tags were used, return a pessimistic ambiguity value.
+  if (est.targetsUsed == null || est.targetsUsed.isEmpty()) return 1.0;
 
-  private boolean agrees(Pose2d posA, Pose2d posB, double maxXY, double maxThetaRad) {
-    return posA.getTranslation().getDistance(posB.getTranslation()) < maxXY
-        && Math.abs(posA.getRotation().minus(posB.getRotation()).getRadians()) < maxThetaRad;
+  double max = 0.0;
+
+  /*
+   * Track the worst ambiguity among all tags contributing to this estimate.
+   * Using the maximum ambiguity is intentionally conservative.
+   */
+  for (var target : est.targetsUsed) {
+    max = Math.max(max, target.getPoseAmbiguity());
   }
 
-  private List<VisionObservation> filterByConsensus(List<VisionObservation> obs) {
-    List<VisionObservation> accepted = new ArrayList<>();
+  return max;
+}
 
-    for (VisionObservation obsA : obs) {
-      int agreements = 0;
-      for (VisionObservation obsB : obs) {
-        if (obsA == obsB) continue;
-        if (agrees(obsA.pose, obsB.pose, 0.6, Math.toRadians(15))) {
-          agreements++;
-        }
-      }
+/**
+ * Determines whether two robot poses agree closely enough to be considered consistent.
+ *
+ * <p>Agreement is based on both translation and rotation:
+ * <ul>
+ *   <li>The XY distance between the poses must be less than {@code maxXY}</li>
+ *   <li>The angular difference between the poses must be less than {@code maxThetaRad}</li>
+ * </ul>
+ *
+ * @param posA the first pose
+ * @param posB the second pose
+ * @param maxXY the maximum allowed translational difference in meters
+ * @param maxThetaRad the maximum allowed rotational difference in radians
+ * @return {@code true} if the poses are within both tolerances, otherwise {@code false}
+ */
+private boolean agrees(Pose2d posA, Pose2d posB, double maxXY, double maxThetaRad) {
+  return posA.getTranslation().getDistance(posB.getTranslation()) < maxXY
+      && Math.abs(posA.getRotation().minus(posB.getRotation()).getRadians()) < maxThetaRad;
+}
+
+/**
+ * Filters a list of vision observations by requiring cross-camera agreement or strong multitag support.
+ *
+ * <p>An observation is accepted if either:
+ * <ul>
+ *   <li>At least one other camera reports a sufficiently similar pose, or</li>
+ *   <li>The observation is a strong multitag estimate</li>
+ * </ul>
+ *
+ * <p>This helps reject outliers from single-camera failures while still allowing good multitag
+ * estimates to pass through even if no other camera currently agrees.
+ *
+ * @param obs the raw list of vision observations
+ * @return a filtered list containing only observations that passed consensus checks
+ */
+private List<VisionObservation> filterByConsensus(List<VisionObservation> obs) {
+  List<VisionObservation> accepted = new ArrayList<>();
+
+  /*
+   * Compare every observation against every other observation.
+   * If another camera agrees with it, it gains support and is more likely to be valid.
+   */
+  for (VisionObservation obsA : obs) {
+    int agreements = 0;
+
+    for (VisionObservation obsB : obs) {
+      // Skip comparing an observation to itself.
+      if (obsA == obsB) continue;
 
-      // keep if another camera agrees, or if it is strong multitag
-      if (agreements > 0 || (obsA.isMultiTag && obsA.tagCount >= 2 && obsA.avgTagDistance < 4.0)) {
-        accepted.add(obsA);
+      /*
+       * Count how many other observations are spatially consistent with this one.
+       * The thresholds here define what "agreement" means across cameras.
+       */
+      if (agrees(obsA.pose, obsB.pose, 0.6, Math.toRadians(15))) {
+        agreements++;
       }
     }
 
-    return accepted;
+    /*
+     * Keep the observation if:
+     * 1. At least one other camera agrees with it, indicating cross-camera consensus, or
+     * 2. It is a strong multitag result, which is generally more trustworthy on its own.
+     */
+    if (agreements > 0 || (obsA.isMultiTag && obsA.tagCount >= 2 && obsA.avgTagDistance < 4.0)) {
+      accepted.add(obsA);
+    }
   }
 
-  private boolean shouldAcceptEnabledMeasurement(
-      VisionObservation obs,
-      Pose2d currentPose,
-      ChassisSpeeds speeds,
-      int supportCount) {
+  return accepted;
+}
 
-    double translationalError =
-        currentPose.getTranslation().getDistance(obs.pose.getTranslation());
-    double headingError =
-        Math.abs(currentPose.getRotation().minus(obs.pose.getRotation()).getRadians());
+/**
+ * Determines whether a vision measurement should be accepted while the robot is enabled.
+ *
+ * <p>When enabled, robot motion and odometry updates are actively occurring, so blindly accepting
+ * large pose jumps can destabilize localization. This method allows:
+ * <ul>
+ *   <li>Measurements close to the current pose estimate</li>
+ *   <li>Larger corrections if the vision is especially strong</li>
+ *   <li>Larger corrections if multiple observations support the measurement</li>
+ * </ul>
+ *
+ * @param obs the vision observation being considered
+ * @param currentPose the robot's current estimated pose from odometry / pose estimation
+ * @param speeds the robot's current chassis speeds
+ * @param supportCount the number of accepted observations supporting the current vision update cycle
+ * @return {@code true} if the measurement should be applied, otherwise {@code false}
+ */
+private boolean shouldAcceptEnabledMeasurement(
+    VisionObservation obs,
+    Pose2d currentPose,
+    ChassisSpeeds speeds,
+    int supportCount) {
 
-    double speed = Math.hypot(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond);
+  // Compute translational and angular error between the current estimate and vision.
+  double translationalError =
+      currentPose.getTranslation().getDistance(obs.pose.getTranslation());
+  double headingError =
+      Math.abs(currentPose.getRotation().minus(obs.pose.getRotation()).getRadians());
 
-    // Base tolerance
-    double maxTranslationJump = 0.5 + 0.35 * speed;
-    double maxHeadingJump = Math.toRadians(20);
+  // Use planar speed magnitude to scale allowable positional correction.
+  double speed = Math.hypot(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond);
 
-    // Strong multitag or multi-camera consensus may correct larger errors
-    boolean strongVision = obs.isMultiTag && obs.avgTagDistance < 4.0;
-    boolean hasSupport = supportCount >= 2;
+  /*
+   * Base tolerance:
+   * - Allow larger translational jumps when the robot is moving faster, since odometry can drift
+   *   more under motion.
+   * - Keep heading jump tolerance fixed for now.
+   */
+  double maxTranslationJump = 0.5 + 0.35 * speed;
+  double maxHeadingJump = Math.toRadians(20);
 
-    if (translationalError < maxTranslationJump && headingError < maxHeadingJump) {
-      return true;
-    }
+  /*
+   * Define conditions under which a larger-than-normal correction is acceptable:
+   * - strongVision: multitag solution with reasonably close tags
+   * - hasSupport: multiple accepted observations this cycle
+   */
+  boolean strongVision = obs.isMultiTag && obs.avgTagDistance < 4.0;
+  boolean hasSupport = supportCount >= 2;
 
-    return strongVision || hasSupport;
+  // Accept small, ordinary corrections immediately.
+  if (translationalError < maxTranslationJump && headingError < maxHeadingJump) {
+    return true;
   }
 
-  /**
-   * Update the pose estimation inside of {@link SwerveDrive} with all of the
-   * given poses.
-   *
-   * @param swerveDrive {@link SwerveDrive} instance.
+  /*
+   * Otherwise, only accept the correction if the vision data is especially trustworthy
+   * or multiple observations support it.
    */
-  public void updatePoseEstimation(SwerveDrive swerveDrive) {
-    if (SwerveDriveTelemetry.isSimulation && swerveDrive.getSimulationDriveTrainPose().isPresent()) {
-      /*
-       * In the maple-sim, odometry is simulated using encoder values, accounting for
-       * factors like skidding and drifting.
-       * As a result, the odometry may not always be 100% accurate.
-       * However, the vision system should be able to provide a reasonably accurate
-       * pose estimation, even when odometry is incorrect.
-       * (This is why teams implement vision system to correct odometry.)
-       * Therefore, we must ensure that the actual robot pose is provided in the
-       * simulator when updating the vision simulation during the simulation.
-       */
-      visionSim.update(swerveDrive.getSimulationDriveTrainPose().get());
-    }
-    
-    // Get the robot's current position for reference.
-    Pose2d currentPose = swerveDrive.getPose();
-    ChassisSpeeds speeds = swerveDrive.getRobotVelocity();
-
-    List<VisionObservation> observations = new ArrayList<>();
-    for (Cameras camera : Cameras.values()) {
-      Optional<EstimatedRobotPose> poseEst = getEstimatedGlobalPose(camera);
-      if (!poseEst.isPresent()) continue; 
-
-      EstimatedRobotPose estPos =  poseEst.get();
-      Pose2d estPose2d = estPos.estimatedPose.toPose2d();
-      
-      int tagsUsed = estPos.targetsUsed == null ? 0 : estPos.targetsUsed.size();
-      if (tagsUsed == 0) continue;
-
-      double avgDist = averageTagDistance(estPos);
-      double maxAmbiguity = maxAmbiguity(estPos);
-      boolean isMultiTag = tagsUsed >= 2;
-      Matrix<N3, N1> currStdDevs = camera.curStdDevs;
-
-      observations.add(new VisionObservation(
-        camera,
-        estPos,
-        estPose2d, 
-        maxAmbiguity, 
-        tagsUsed, 
-        avgDist, 
-        maxAmbiguity, 
-        isMultiTag, 
-        currStdDevs
-        ));
-    }
+  return strongVision || hasSupport;
+}
+
+/**
+ * Updates the robot pose estimator with valid vision measurements from all configured cameras.
+ *
+ * <p>This method performs the following high-level steps:
+ * <ol>
+ *   <li>Updates the vision simulator during simulation</li>
+ *   <li>Collects pose estimates from each camera</li>
+ *   <li>Builds {@link VisionObservation} objects with quality metadata</li>
+ *   <li>Filters out inconsistent observations using cross-camera consensus</li>
+ *   <li>Falls back to multitag-only observations if consensus fails</li>
+ *   <li>Applies accepted measurements to the swerve drive pose estimator</li>
+ * </ol>
+ *
+ * <p>The goal is to make vision updates robust against bad individual camera measurements while
+ * still allowing strong observations to correct odometry drift.
+ *
+ * @param swerveDrive the {@link SwerveDrive} instance whose pose estimator should be updated
+ */
+public void updatePoseEstimation(SwerveDrive swerveDrive) {
+  if (SwerveDriveTelemetry.isSimulation && swerveDrive.getSimulationDriveTrainPose().isPresent()) {
+    /*
+     * In simulation, the drivetrain pose used by the simulator represents the "ground truth"
+     * position of the robot.
+     *
+     * Simulated odometry may still include effects such as skidding or drift, meaning the pose
+     * estimator's internal odometry can deviate from truth just like on a real robot.
+     *
+     * Since vision exists specifically to correct odometry drift, the vision simulator should
+     * be updated using the true simulated robot pose rather than the potentially drifted estimate.
+     */
+    visionSim.update(swerveDrive.getSimulationDriveTrainPose().get());
+  }
 
-    if (observations.isEmpty()) return;
+  // Read the robot's current estimated pose and velocity for gating future measurements.
+  Pose2d currentPose = swerveDrive.getPose();
+  ChassisSpeeds speeds = swerveDrive.getRobotVelocity();
 
-    // Reject inconsistent outliers using cross-camera agreement
-    List<VisionObservation> filtered = filterByConsensus(observations);
+  List<VisionObservation> observations = new ArrayList<>();
 
-    if (filtered.isEmpty()) {
-      filtered = observations.stream()
-          .filter(o -> o.isMultiTag)
-          .toList();
-    }
+  /*
+   * Query every configured camera for a global pose estimate.
+   * Each valid estimate is converted into a VisionObservation containing
+   * pose data plus metadata used for filtering and trust evaluation.
+   */
+  for (Cameras camera : Cameras.values()) {
+    Optional<EstimatedRobotPose> poseEst = getEstimatedGlobalPose(camera);
+
+    // Skip cameras that do not currently have a valid estimate.
+    if (!poseEst.isPresent()) continue;
+
+    EstimatedRobotPose estPos = poseEst.get();
+    Pose2d estPose2d = estPos.estimatedPose.toPose2d();
+
+    // Determine how many tags contributed to this estimate.
+    int tagsUsed = estPos.targetsUsed == null ? 0 : estPos.targetsUsed.size();
+
+    // Ignore estimates that were not built from any tags.
+    if (tagsUsed == 0) continue;
+
+    // Compute quality metrics used later for filtering and acceptance decisions.
+    double avgDist = averageTagDistance(estPos);
+    double maxAmbiguity = maxAmbiguity(estPos);
+    boolean isMultiTag = tagsUsed >= 2;
+    Matrix<N3, N1> currStdDevs = camera.curStdDevs;
+
+    /*
+     * Store a normalized observation record for later filtering.
+     * This keeps the later pipeline cleaner by collecting all relevant
+     * metadata in one place.
+     */
+    observations.add(new VisionObservation(
+      camera,
+      estPos,
+      estPose2d,
+      estPos.timestampSeconds,
+      tagsUsed,
+      avgDist,
+      maxAmbiguity,
+      isMultiTag,
+      currStdDevs
+    ));
+  }
 
-    if (filtered.isEmpty()) {
-      return;
-    }
+  // Nothing to do if no camera produced a usable observation.
+  if (observations.isEmpty()) return;
 
-    // Update the robot's position using the valid position observations.
-    for (VisionObservation obs : filtered) {
-      if (shouldAcceptEnabledMeasurement(obs, currentPose, speeds, filtered.size())) {
-        swerveDrive.addVisionMeasurement(obs.pose, obs.timestamp, obs.stdDevs);
-      }
-    } 
+  // First-pass rejection of outliers using cross-camera agreement.
+  List<VisionObservation> filtered = filterByConsensus(observations);
+
+  if (filtered.isEmpty()) {
+    /*
+     * If no observations achieved consensus, fall back to multitag-only observations.
+     * This gives strong single-camera multitag estimates a chance to still correct pose.
+     */
+    filtered = observations.stream()
+        .filter(o -> o.isMultiTag)
+        .toList();
   }
 
+  // If nothing survived filtering or fallback, skip this update cycle entirely.
+  if (filtered.isEmpty()) {
+    return;
+  }
+
+  /*
+   * Feed accepted vision measurements into the drivetrain pose estimator.
+   * Each measurement is individually gated against current pose and robot motion.
+   */
+  for (VisionObservation obs : filtered) {
+    if (shouldAcceptEnabledMeasurement(obs, currentPose, speeds, filtered.size())) {
+      swerveDrive.addVisionMeasurement(obs.pose, obs.timestamp, obs.stdDevs);
+    }
+  }
+}
+
   /**
    * Generates the estimated robot pose. Returns empty if:
    * <ul>

From aa1983be14bc1cf26fe97b2c8c0130b1f3aab353 Mon Sep 17 00:00:00 2001
From: N/A <cosmicaura3@gmail.com>
Date: Tue, 17 Mar 2026 18:46:46 -0500
Subject: [PATCH 04/26] refactor: clean up redundant imports

---
 .vscode/settings.json                                      | 1 +
 .../frc/robot/subsystems/vision/PhotonVisionOdometry.java  | 7 -------
 2 files changed, 1 insertion(+), 7 deletions(-)

diff --git a/.vscode/settings.json b/.vscode/settings.json
index 935e249..4068b73 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -65,6 +65,7 @@
     "gson",
     "Holonomic",
     "Jetson",
+    "multitag",
     "PATHFIND",
     "Pathfinding",
     "Pathfinds",
diff --git a/src/main/java/frc/robot/subsystems/vision/PhotonVisionOdometry.java b/src/main/java/frc/robot/subsystems/vision/PhotonVisionOdometry.java
index 31eaea2..dcabecb 100644
--- a/src/main/java/frc/robot/subsystems/vision/PhotonVisionOdometry.java
+++ b/src/main/java/frc/robot/subsystems/vision/PhotonVisionOdometry.java
@@ -6,14 +6,10 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Transform2d;
-import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
-import edu.wpi.first.units.measure.Distance;
-import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.Robot;
 import frc.robot.util.vision.Cameras;
 import frc.robot.util.vision.VisionObservation;
@@ -25,13 +21,10 @@
 import java.util.function.Supplier;
 import org.photonvision.EstimatedRobotPose;
 import org.photonvision.PhotonUtils;
-import org.photonvision.PhotonPoseEstimator.PoseStrategy;
 import org.photonvision.simulation.VisionSystemSim;
 import org.photonvision.targeting.PhotonPipelineResult;
 import org.photonvision.targeting.PhotonTrackedTarget;
 
-import com.fasterxml.jackson.databind.ser.impl.PropertySerializerMap;
-
 import swervelib.SwerveDrive;
 import swervelib.telemetry.SwerveDriveTelemetry;
 

From f900fae996faf4f570d9a606211dee9bcf62012f Mon Sep 17 00:00:00 2001
From: Cameron Myhre <cameronmyhre@icloud.com>
Date: Tue, 17 Mar 2026 20:00:45 -0500
Subject: [PATCH 05/26] refactor: rename variable for clarity

Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com>
---
 .../frc/robot/subsystems/vision/PhotonVisionOdometry.java     | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/vision/PhotonVisionOdometry.java b/src/main/java/frc/robot/subsystems/vision/PhotonVisionOdometry.java
index dcabecb..77f8bb5 100644
--- a/src/main/java/frc/robot/subsystems/vision/PhotonVisionOdometry.java
+++ b/src/main/java/frc/robot/subsystems/vision/PhotonVisionOdometry.java
@@ -358,7 +358,7 @@ public void updatePoseEstimation(SwerveDrive swerveDrive) {
 
     // Compute quality metrics used later for filtering and acceptance decisions.
     double avgDist = averageTagDistance(estPos);
-    double maxAmbiguity = maxAmbiguity(estPos);
+    double maxTagAmbiguity = maxAmbiguity(estPos);
     boolean isMultiTag = tagsUsed >= 2;
     Matrix<N3, N1> currStdDevs = camera.curStdDevs;
 
@@ -374,7 +374,7 @@ public void updatePoseEstimation(SwerveDrive swerveDrive) {
       estPos.timestampSeconds,
       tagsUsed,
       avgDist,
-      maxAmbiguity,
+      maxTagAmbiguity,
       isMultiTag,
       currStdDevs
     ));

From a5730251af63865f7d8a907d8dfc383b42955b85 Mon Sep 17 00:00:00 2001
From: Cameron Myhre <cameronmyhre@icloud.com>
Date: Tue, 17 Mar 2026 20:02:03 -0500
Subject: [PATCH 06/26] feat: gate by ambiguity

Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com>
---
 .../frc/robot/subsystems/vision/PhotonVisionOdometry.java    | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/src/main/java/frc/robot/subsystems/vision/PhotonVisionOdometry.java b/src/main/java/frc/robot/subsystems/vision/PhotonVisionOdometry.java
index 77f8bb5..ec63b07 100644
--- a/src/main/java/frc/robot/subsystems/vision/PhotonVisionOdometry.java
+++ b/src/main/java/frc/robot/subsystems/vision/PhotonVisionOdometry.java
@@ -362,6 +362,11 @@ public void updatePoseEstimation(SwerveDrive swerveDrive) {
     boolean isMultiTag = tagsUsed >= 2;
     Matrix<N3, N1> currStdDevs = camera.curStdDevs;
 
+    // Reject highly ambiguous estimates before they enter the consensus pipeline.
+    if (maxAmbiguity > maximumAmbiguity) {
+      continue;
+    }
+
     /*
      * Store a normalized observation record for later filtering.
      * This keeps the later pipeline cleaner by collecting all relevant

From d9d29037ea4dd91f08659624f21c835ebd70e9db Mon Sep 17 00:00:00 2001
From: Cameron Myhre <cameronmyhre@icloud.com>
Date: Tue, 17 Mar 2026 20:04:19 -0500
Subject: [PATCH 07/26] docs: fix ai written comment

Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com>
---
 .../frc/robot/subsystems/vision/PhotonVisionOdometry.java   | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/vision/PhotonVisionOdometry.java b/src/main/java/frc/robot/subsystems/vision/PhotonVisionOdometry.java
index ec63b07..cf98bdc 100644
--- a/src/main/java/frc/robot/subsystems/vision/PhotonVisionOdometry.java
+++ b/src/main/java/frc/robot/subsystems/vision/PhotonVisionOdometry.java
@@ -237,10 +237,10 @@ private List<VisionObservation> filterByConsensus(List<VisionObservation> obs) {
 }
 
 /**
- * Determines whether a vision measurement should be accepted while the robot is enabled.
+ * Determines whether a vision measurement should be accepted given the current robot state.
  *
- * <p>When enabled, robot motion and odometry updates are actively occurring, so blindly accepting
- * large pose jumps can destabilize localization. This method allows:
+ * <p>Robot motion and odometry updates are actively occurring, so blindly accepting large pose
+ * jumps can destabilize localization. This method allows:
  * <ul>
  *   <li>Measurements close to the current pose estimate</li>
  *   <li>Larger corrections if the vision is especially strong</li>

From c32c6253e1af20ba5ca63e14baf841e0b7918784 Mon Sep 17 00:00:00 2001
From: Cameron Myhre <cameronmyhre@icloud.com>
Date: Tue, 17 Mar 2026 20:05:30 -0500
Subject: [PATCH 08/26] refactor: improve maintainability of hardcoded
 constants

Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com>
---
 .../subsystems/vision/PhotonVisionOdometry.java  | 16 ++++++++++++++--
 1 file changed, 14 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/vision/PhotonVisionOdometry.java b/src/main/java/frc/robot/subsystems/vision/PhotonVisionOdometry.java
index cf98bdc..c11f2d0 100644
--- a/src/main/java/frc/robot/subsystems/vision/PhotonVisionOdometry.java
+++ b/src/main/java/frc/robot/subsystems/vision/PhotonVisionOdometry.java
@@ -35,6 +35,11 @@
  */
 public class PhotonVisionOdometry {
 
+  // Tuning constants for cross-camera consensus and multitag filtering.
+  private static final double CONSENSUS_MAX_TRANSLATION_METERS = 0.6;
+  private static final double CONSENSUS_MAX_ROTATION_RADIANS = Math.toRadians(15.0);
+  private static final double STRONG_MULTITAG_MAX_DISTANCE_METERS = 4.0;
+
   /**
    * April Tag Field Layout of the year.
    */
@@ -218,7 +223,11 @@ private List<VisionObservation> filterByConsensus(List<VisionObservation> obs) {
        * Count how many other observations are spatially consistent with this one.
        * The thresholds here define what "agreement" means across cameras.
        */
-      if (agrees(obsA.pose, obsB.pose, 0.6, Math.toRadians(15))) {
+      if (agrees(
+          obsA.pose,
+          obsB.pose,
+          CONSENSUS_MAX_TRANSLATION_METERS,
+          CONSENSUS_MAX_ROTATION_RADIANS)) {
         agreements++;
       }
     }
@@ -228,7 +237,10 @@ private List<VisionObservation> filterByConsensus(List<VisionObservation> obs) {
      * 1. At least one other camera agrees with it, indicating cross-camera consensus, or
      * 2. It is a strong multitag result, which is generally more trustworthy on its own.
      */
-    if (agreements > 0 || (obsA.isMultiTag && obsA.tagCount >= 2 && obsA.avgTagDistance < 4.0)) {
+    if (agreements > 0
+        || (obsA.isMultiTag
+            && obsA.tagCount >= 2
+            && obsA.avgTagDistance < STRONG_MULTITAG_MAX_DISTANCE_METERS)) {
       accepted.add(obsA);
     }
   }

From 9f558b1f805b2f123f140165968f008baf4dfca4 Mon Sep 17 00:00:00 2001
From: Cameron Myhre <cameronmyhre@icloud.com>
Date: Tue, 17 Mar 2026 20:07:09 -0500
Subject: [PATCH 09/26] feat: improve consensus filtering with agreement count

Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com>
---
 .../vision/PhotonVisionOdometry.java          | 37 ++++++++++++++++++-
 1 file changed, 35 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/vision/PhotonVisionOdometry.java b/src/main/java/frc/robot/subsystems/vision/PhotonVisionOdometry.java
index c11f2d0..cc62635 100644
--- a/src/main/java/frc/robot/subsystems/vision/PhotonVisionOdometry.java
+++ b/src/main/java/frc/robot/subsystems/vision/PhotonVisionOdometry.java
@@ -418,12 +418,45 @@ public void updatePoseEstimation(SwerveDrive swerveDrive) {
     return;
   }
 
+  /*
+   * Compute per-observation support: for each observation, count how many other
+   * observations have a sufficiently similar pose (i.e., they agree). This avoids
+   * treating all multitag observations as mutually supporting when they disagree.
+   */
+  int n = filtered.size();
+  List<Integer> supportCounts = new ArrayList<>(n);
+  for (int i = 0; i < n; i++) {
+    // Start each observation with a support count of 1 (itself).
+    supportCounts.add(1);
+  }
+
+  // Thresholds for considering two observations to be in agreement.
+  final double translationEpsMeters = 0.5; // positional agreement threshold
+  final double rotationEpsRadians = Math.toRadians(10.0); // angular agreement threshold
+
+  for (int i = 0; i < n; i++) {
+    Pose2d poseI = filtered.get(i).pose;
+    for (int j = i + 1; j < n; j++) {
+      Pose2d poseJ = filtered.get(j).pose;
+      Transform2d delta = poseI.minus(poseJ);
+      double dist = delta.getTranslation().getNorm();
+      double rotDiff = Math.abs(delta.getRotation().getRadians());
+      if (dist <= translationEpsMeters && rotDiff <= rotationEpsRadians) {
+        // Observations i and j agree; increment support for both.
+        supportCounts.set(i, supportCounts.get(i) + 1);
+        supportCounts.set(j, supportCounts.get(j) + 1);
+      }
+    }
+  }
+
   /*
    * Feed accepted vision measurements into the drivetrain pose estimator.
    * Each measurement is individually gated against current pose and robot motion.
    */
-  for (VisionObservation obs : filtered) {
-    if (shouldAcceptEnabledMeasurement(obs, currentPose, speeds, filtered.size())) {
+  for (int i = 0; i < n; i++) {
+    VisionObservation obs = filtered.get(i);
+    int support = supportCounts.get(i);
+    if (shouldAcceptEnabledMeasurement(obs, currentPose, speeds, support)) {
       swerveDrive.addVisionMeasurement(obs.pose, obs.timestamp, obs.stdDevs);
     }
   }

From 7c4d05bc97c7a94de022d1060b7e2aa938ff7172 Mon Sep 17 00:00:00 2001
From: N/A <cosmicaura3@gmail.com>
Date: Tue, 17 Mar 2026 20:09:34 -0500
Subject: [PATCH 10/26] fix: correct copilot variable naming issue

---
 .../java/frc/robot/subsystems/vision/PhotonVisionOdometry.java  | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/vision/PhotonVisionOdometry.java b/src/main/java/frc/robot/subsystems/vision/PhotonVisionOdometry.java
index cc62635..b97afad 100644
--- a/src/main/java/frc/robot/subsystems/vision/PhotonVisionOdometry.java
+++ b/src/main/java/frc/robot/subsystems/vision/PhotonVisionOdometry.java
@@ -375,7 +375,7 @@ public void updatePoseEstimation(SwerveDrive swerveDrive) {
     Matrix<N3, N1> currStdDevs = camera.curStdDevs;
 
     // Reject highly ambiguous estimates before they enter the consensus pipeline.
-    if (maxAmbiguity > maximumAmbiguity) {
+    if (maxTagAmbiguity > maximumAmbiguity) {
       continue;
     }
 

From 4dd715431b22e2acb202c42b5b86ecf4b0faaa16 Mon Sep 17 00:00:00 2001
From: "Jason M. Dudley" <j.m.duds@gmail.com>
Date: Sat, 28 Mar 2026 09:18:53 -0500
Subject: [PATCH 11/26] Test commit

---
 src/main/java/frc/robot/subsystems/IntakePivotSubsystem.java | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/main/java/frc/robot/subsystems/IntakePivotSubsystem.java b/src/main/java/frc/robot/subsystems/IntakePivotSubsystem.java
index 0926be7..7daf779 100644
--- a/src/main/java/frc/robot/subsystems/IntakePivotSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/IntakePivotSubsystem.java
@@ -15,6 +15,7 @@
 import frc.robot.Configs;
 import frc.robot.Constants;
 
+// Test
 public class IntakePivotSubsystem extends SubsystemBase {
     public TalonFX intake;
     private final PositionVoltage angleRequest = new PositionVoltage(0);

From cf991c614decbddc9dc306429a480acbce8df254 Mon Sep 17 00:00:00 2001
From: N/A <cosmicaura3@gmail.com>
Date: Sat, 28 Mar 2026 12:45:11 -0500
Subject: [PATCH 12/26] fix: add 2s delay to auto to let shooter rev up and
 prevent jams

---
 src/main/java/frc/robot/commands/ShootPreloadCommand.java | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc/robot/commands/ShootPreloadCommand.java b/src/main/java/frc/robot/commands/ShootPreloadCommand.java
index 8bde4fd..ed51c7b 100644
--- a/src/main/java/frc/robot/commands/ShootPreloadCommand.java
+++ b/src/main/java/frc/robot/commands/ShootPreloadCommand.java
@@ -5,10 +5,9 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-import frc.robot.Constants;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
 import frc.robot.subsystems.IntakePivotSubsystem;
 import frc.robot.subsystems.IntakeRollerSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
@@ -32,9 +31,10 @@ public ShootPreloadCommand(
         addCommands(
 
             Commands.deadline(
-                Commands.waitSeconds(shootPreloadTime),
+                Commands.waitSeconds(shootPreloadTime + 2),
                 turret.getTargetCommand(targetSupplier, robotPoseSupplier, robotVelocitySupplier),
                 shooter.getTargetCommand(targetSupplier, robotPoseSupplier, robotVelocitySupplier),
+                new WaitCommand(2), // 
                 transfer.getLoadCommand(),
                 intakePivot.deployIntakeCommand(),
                 intakeRoller.getIntakeCommand()

From 82b1f60ab16c7c66458919dee51f0bc57cc52af9 Mon Sep 17 00:00:00 2001
From: N/A <cosmicaura3@gmail.com>
Date: Sat, 28 Mar 2026 14:44:46 -0500
Subject: [PATCH 13/26] feat(auto): deploy intake before shooting delay

---
 src/main/java/frc/robot/commands/ShootPreloadCommand.java | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc/robot/commands/ShootPreloadCommand.java b/src/main/java/frc/robot/commands/ShootPreloadCommand.java
index ed51c7b..98237a0 100644
--- a/src/main/java/frc/robot/commands/ShootPreloadCommand.java
+++ b/src/main/java/frc/robot/commands/ShootPreloadCommand.java
@@ -34,10 +34,10 @@ public ShootPreloadCommand(
                 Commands.waitSeconds(shootPreloadTime + 2),
                 turret.getTargetCommand(targetSupplier, robotPoseSupplier, robotVelocitySupplier),
                 shooter.getTargetCommand(targetSupplier, robotPoseSupplier, robotVelocitySupplier),
-                new WaitCommand(2), // 
-                transfer.getLoadCommand(),
+                intakeRoller.getIntakeCommand(),
                 intakePivot.deployIntakeCommand(),
-                intakeRoller.getIntakeCommand()
+                new WaitCommand(2), // 
+                transfer.getLoadCommand()
             ),
 
             Commands.parallel(

From 4900911e0fc0efe552bf562318fa11f9960ad1c1 Mon Sep 17 00:00:00 2001
From: N/A <cosmicaura3@gmail.com>
Date: Sat, 28 Mar 2026 14:46:32 -0500
Subject: [PATCH 14/26] fix: flip depot and outpost autos when on red

---
 src/main/java/frc/robot/RobotContainer.java | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index e402363..c7ca529 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -587,8 +587,8 @@ private Command getIntakeStrategyCommand() {
             turretSubsystem,
             transferSubsystem,
             () -> FieldUtil.flipIfRed(Constants.Field.BLUE_HUB_TRANSLATION),
-            Constants.Operator.Auto.DEPOT_READY_INTAKE_POSE,
-            Constants.Operator.Auto.DEPOT_INTAKE_POSE,
+            FieldUtil.flipIfRed(Constants.Operator.Auto.DEPOT_READY_INTAKE_POSE),
+            FieldUtil.flipIfRed(Constants.Operator.Auto.DEPOT_INTAKE_POSE),
             false,
             Constants.Operator.Auto.AUTO_INTAKE_MAX_SPEED,
             SmartDashboard.getNumber("Auto/IntakeShootTime", Constants.Operator.Auto.DEFAULT_INTAKE_SHOOT_TIME));
@@ -602,8 +602,8 @@ private Command getIntakeStrategyCommand() {
             turretSubsystem,
             transferSubsystem,
             () -> FieldUtil.flipIfRed(Constants.Field.BLUE_HUB_TRANSLATION),
-            Constants.Operator.Auto.OUTPOST_READY_INTAKE_POSE,
-            Constants.Operator.Auto.OUTPOST_INTAKE_POSE,
+            FieldUtil.flipIfRed(Constants.Operator.Auto.OUTPOST_READY_INTAKE_POSE),
+            FieldUtil.flipIfRed(Constants.Operator.Auto.OUTPOST_INTAKE_POSE),
             false,
             Constants.Operator.Auto.AUTO_INTAKE_MAX_SPEED,
             SmartDashboard.getNumber("Auto/IntakeShootTime", Constants.Operator.Auto.DEFAULT_INTAKE_SHOOT_TIME));

From e7be1897006b8f8dfcf2253af5cfffbfb48b6797 Mon Sep 17 00:00:00 2001
From: "Jason M. Dudley" <j.m.duds@gmail.com>
Date: Sat, 28 Mar 2026 14:59:29 -0500
Subject: [PATCH 15/26] Increase power offset rate.  Add buttons to reset
 offsets.

---
 src/main/java/frc/robot/Constants.java      |  2 +-
 src/main/java/frc/robot/RobotContainer.java | 24 +++++++++++++++++++++
 2 files changed, 25 insertions(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 2c51cc7..99ca347 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -58,7 +58,7 @@ public static class ErrorSettings {
             public static final int TURRET_OFFSET_DECIMAL_PLACE = 1;
             public static final double HOOD_OFFSET_INCREASE = 0.1;
             public static final int HOOD_OFFSET_DECIMAL_PLACE = 1;
-            public static final double SHOOTER_PERCENT_INCREASE = 0.2; // Percent to increase per tick
+            public static final double SHOOTER_PERCENT_INCREASE = 2; // Percent to increase per tick
             public static final int SHOOTER_PERCENT_DECIMAL_PLACE = 1;
 
             public static final double SHOOTER_PERCENT_DEFAULT = 100.0;
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index e402363..29ca267 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -388,6 +388,30 @@ public void configureBindings() {
             shooterPercent.getHeldIntervalCommand(Constants.Operator.ErrorSettings.SHOOTER_PERCENT_INCREASE,
                 Constants.Operator.ErrorSettings.SETTINGS_DELAY_TIME));
 
+    // ==============================
+    // Turret / Hood Offset Reset (Left Stick Click / Right Stick Click)
+    // ==============================
+
+    // Left Stick Click : Reset turret offset to zero
+    new Trigger(() -> operatorController.getLeftStickButton())
+        .onTrue(new InstantCommand(() -> turretOffsetDegrees.set(0.0)));
+
+    // Right Stick Click : Reset hood offset to zero
+    new Trigger(() -> operatorController.getRightStickButton())
+        .onTrue(new InstantCommand(() -> hoodOffsetDegrees.set(0.0)));
+
+    // ==============================
+    // Shooter Percent Reset (Left Trigger / Right Trigger)
+    // ==============================
+
+    // Left Trigger : Reset shooter percent to default
+    new Trigger(() -> operatorController.getLeftTriggerAxis() > 0.5)
+        .onTrue(new InstantCommand(() -> shooterPercent.set(Constants.Operator.ErrorSettings.SHOOTER_PERCENT_DEFAULT)));
+
+    // Right Trigger : Reset shooter percent to default
+    new Trigger(() -> operatorController.getRightTriggerAxis() > 0.5)
+        .onTrue(new InstantCommand(() -> shooterPercent.set(Constants.Operator.ErrorSettings.SHOOTER_PERCENT_DEFAULT)));
+
     new Trigger(
         () -> operatorController.getAButton()).onTrue(
             new InstantCommand(() -> selectedFixedTarget = FixedTarget.A));

From 249cb68a4b68e6455b728860f6a637a1dc748c98 Mon Sep 17 00:00:00 2001
From: "Jason M. Dudley" <j.m.duds@gmail.com>
Date: Sat, 28 Mar 2026 15:03:06 -0500
Subject: [PATCH 16/26] Remove test comment

---
 src/main/java/frc/robot/subsystems/IntakePivotSubsystem.java | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/IntakePivotSubsystem.java b/src/main/java/frc/robot/subsystems/IntakePivotSubsystem.java
index 7daf779..0926be7 100644
--- a/src/main/java/frc/robot/subsystems/IntakePivotSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/IntakePivotSubsystem.java
@@ -15,7 +15,6 @@
 import frc.robot.Configs;
 import frc.robot.Constants;
 
-// Test
 public class IntakePivotSubsystem extends SubsystemBase {
     public TalonFX intake;
     private final PositionVoltage angleRequest = new PositionVoltage(0);

From cc4c03480ec92fe70769b2a9cefe1609deae8344 Mon Sep 17 00:00:00 2001
From: "Jason M. Dudley" <j.m.duds@gmail.com>
Date: Sat, 28 Mar 2026 15:15:42 -0500
Subject: [PATCH 17/26] Drop to 1 percent per tick for shooter percentage

---
 src/main/java/frc/robot/Constants.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 99ca347..1a3b8ca 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -58,7 +58,7 @@ public static class ErrorSettings {
             public static final int TURRET_OFFSET_DECIMAL_PLACE = 1;
             public static final double HOOD_OFFSET_INCREASE = 0.1;
             public static final int HOOD_OFFSET_DECIMAL_PLACE = 1;
-            public static final double SHOOTER_PERCENT_INCREASE = 2; // Percent to increase per tick
+            public static final double SHOOTER_PERCENT_INCREASE = 1; // Percent to increase per tick
             public static final int SHOOTER_PERCENT_DECIMAL_PLACE = 1;
 
             public static final double SHOOTER_PERCENT_DEFAULT = 100.0;

From fc752159cd8c30666bbcbf1b22600312359ac43f Mon Sep 17 00:00:00 2001
From: "Jason M. Dudley" <j.m.duds@gmail.com>
Date: Sat, 28 Mar 2026 15:29:05 -0500
Subject: [PATCH 18/26] Combine reset for both triggers into one command

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>
---
 src/main/java/frc/robot/RobotContainer.java | 10 +++-------
 1 file changed, 3 insertions(+), 7 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 29ca267..e58238e 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -404,14 +404,10 @@ public void configureBindings() {
     // Shooter Percent Reset (Left Trigger / Right Trigger)
     // ==============================
 
-    // Left Trigger : Reset shooter percent to default
-    new Trigger(() -> operatorController.getLeftTriggerAxis() > 0.5)
+    // Either Trigger : Reset shooter percent to default
+    new Trigger(() -> operatorController.getLeftTriggerAxis() > 0.5
+        || operatorController.getRightTriggerAxis() > 0.5)
         .onTrue(new InstantCommand(() -> shooterPercent.set(Constants.Operator.ErrorSettings.SHOOTER_PERCENT_DEFAULT)));
-
-    // Right Trigger : Reset shooter percent to default
-    new Trigger(() -> operatorController.getRightTriggerAxis() > 0.5)
-        .onTrue(new InstantCommand(() -> shooterPercent.set(Constants.Operator.ErrorSettings.SHOOTER_PERCENT_DEFAULT)));
-
     new Trigger(
         () -> operatorController.getAButton()).onTrue(
             new InstantCommand(() -> selectedFixedTarget = FixedTarget.A));

From c7fa2eaadffed2cc3b41aec92c66bad07869f508 Mon Sep 17 00:00:00 2001
From: N/A <cosmicaura3@gmail.com>
Date: Sat, 28 Mar 2026 15:29:45 -0500
Subject: [PATCH 19/26] fix: correct 2 second delay issue

---
 .../frc/robot/commands/ShootPreloadCommand.java | 17 +++++++++++------
 1 file changed, 11 insertions(+), 6 deletions(-)

diff --git a/src/main/java/frc/robot/commands/ShootPreloadCommand.java b/src/main/java/frc/robot/commands/ShootPreloadCommand.java
index 98237a0..2207b2c 100644
--- a/src/main/java/frc/robot/commands/ShootPreloadCommand.java
+++ b/src/main/java/frc/robot/commands/ShootPreloadCommand.java
@@ -6,6 +6,7 @@
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import frc.robot.subsystems.IntakePivotSubsystem;
@@ -32,12 +33,16 @@ public ShootPreloadCommand(
 
             Commands.deadline(
                 Commands.waitSeconds(shootPreloadTime + 2),
-                turret.getTargetCommand(targetSupplier, robotPoseSupplier, robotVelocitySupplier),
-                shooter.getTargetCommand(targetSupplier, robotPoseSupplier, robotVelocitySupplier),
-                intakeRoller.getIntakeCommand(),
-                intakePivot.deployIntakeCommand(),
-                new WaitCommand(2), // 
-                transfer.getLoadCommand()
+                new SequentialCommandGroup(
+                    new ParallelCommandGroup(
+                        turret.getTargetCommand(targetSupplier, robotPoseSupplier, robotVelocitySupplier),
+                        shooter.getTargetCommand(targetSupplier, robotPoseSupplier, robotVelocitySupplier),
+                        intakeRoller.getIntakeCommand(),
+                        intakePivot.deployIntakeCommand()
+                    ),
+                    new WaitCommand(2),
+                    transfer.getLoadCommand()
+                )
             ),
 
             Commands.parallel(

From 6fba5825a7727c0fa6b9a16a4026602fed27c820 Mon Sep 17 00:00:00 2001
From: "Jason M. Dudley" <j.m.duds@gmail.com>
Date: Sat, 28 Mar 2026 15:54:21 -0500
Subject: [PATCH 20/26] 0.5 percent per 20 ms for shooter power

---
 src/main/java/frc/robot/Constants.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 1a3b8ca..5507c65 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -58,7 +58,7 @@ public static class ErrorSettings {
             public static final int TURRET_OFFSET_DECIMAL_PLACE = 1;
             public static final double HOOD_OFFSET_INCREASE = 0.1;
             public static final int HOOD_OFFSET_DECIMAL_PLACE = 1;
-            public static final double SHOOTER_PERCENT_INCREASE = 1; // Percent to increase per tick
+            public static final double SHOOTER_PERCENT_INCREASE = .5; // Percent to increase per tick
             public static final int SHOOTER_PERCENT_DECIMAL_PLACE = 1;
 
             public static final double SHOOTER_PERCENT_DEFAULT = 100.0;

From 6e7e65eaef2643cbde12afb7a80527c88cc88541 Mon Sep 17 00:00:00 2001
From: N/A <cosmicaura3@gmail.com>
Date: Sat, 28 Mar 2026 18:30:18 -0500
Subject: [PATCH 21/26] Revert "Merge branch
 'feature/add-ambiguity-gating-to-vision-odometry' into mukwonago2026"

This reverts commit b6bf6480245146c10be9173a2dfbc5aa11379f53, reversing
changes made to c13a2dd1639dbec5f85c6520ff281a814bc64f9c.
---
 .vscode/settings.json                         |   1 -
 src/main/java/frc/robot/Constants.java        |   1 +
 .../vision/PhotonVisionOdometry.java          | 383 ++----------------
 .../robot/util/vision/VisionObservation.java  |  41 --
 4 files changed, 31 insertions(+), 395 deletions(-)
 delete mode 100644 src/main/java/frc/robot/util/vision/VisionObservation.java

diff --git a/.vscode/settings.json b/.vscode/settings.json
index 4068b73..935e249 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -65,7 +65,6 @@
     "gson",
     "Holonomic",
     "Jetson",
-    "multitag",
     "PATHFIND",
     "Pathfinding",
     "Pathfinds",
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 72e9338..5507c65 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -519,6 +519,7 @@ public static class VisionOdometryConstants {
                 Units.inchesToMeters(9.725),
                 Units.inchesToMeters(20.25));
 
+        long num = 999_999_999;
         public static final Vector<N3> LEFT_SINGLE_TAG_STANDARD_DEVIATION = VecBuilder.fill(4, 4, 8);
         public static final Vector<N3> LEFT_MULTI_TAG_STANDARD_DEVIATION = VecBuilder.fill(0.5, 0.5, 1);
 
diff --git a/src/main/java/frc/robot/subsystems/vision/PhotonVisionOdometry.java b/src/main/java/frc/robot/subsystems/vision/PhotonVisionOdometry.java
index b97afad..c5d54da 100644
--- a/src/main/java/frc/robot/subsystems/vision/PhotonVisionOdometry.java
+++ b/src/main/java/frc/robot/subsystems/vision/PhotonVisionOdometry.java
@@ -2,17 +2,14 @@
 
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.apriltag.AprilTagFields;
-import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Transform2d;
-import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.math.numbers.N1;
-import edu.wpi.first.math.numbers.N3;
+
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.Robot;
 import frc.robot.util.vision.Cameras;
-import frc.robot.util.vision.VisionObservation;
 
 import java.awt.Desktop;
 import java.util.ArrayList;
@@ -24,7 +21,6 @@
 import org.photonvision.simulation.VisionSystemSim;
 import org.photonvision.targeting.PhotonPipelineResult;
 import org.photonvision.targeting.PhotonTrackedTarget;
-
 import swervelib.SwerveDrive;
 import swervelib.telemetry.SwerveDriveTelemetry;
 
@@ -35,11 +31,6 @@
  */
 public class PhotonVisionOdometry {
 
-  // Tuning constants for cross-camera consensus and multitag filtering.
-  private static final double CONSENSUS_MAX_TRANSLATION_METERS = 0.6;
-  private static final double CONSENSUS_MAX_ROTATION_RADIANS = Math.toRadians(15.0);
-  private static final double STRONG_MULTITAG_MAX_DISTANCE_METERS = 4.0;
-
   /**
    * April Tag Field Layout of the year.
    */
@@ -111,356 +102,42 @@ public static Pose2d getAprilTagPose(int aprilTag, Transform2d robotOffset) {
 
   }
   
-  /*
-   * POSITION ESTIMATION
-   */
-/**
- * Computes the average distance from the camera to each AprilTag used in a pose estimate.
- *
- * <p>This is used as a rough quality metric for the estimate. In general, estimates built
- * from closer tags are more trustworthy than estimates built from distant tags, since distance
- * tends to amplify vision noise and pose uncertainty.
- *
- * @param est the estimated robot pose containing the list of AprilTag targets used
- * @return the average camera-to-tag distance in meters, or {@link Double#POSITIVE_INFINITY}
- *     if no targets were used
- */
-private double averageTagDistance(EstimatedRobotPose est) {
-  // If no tags contributed to this estimate, treat the distance as effectively unusable.
-  if (est.targetsUsed == null || est.targetsUsed.isEmpty()) return Double.POSITIVE_INFINITY;
-
-  double sum = 0.0;
-
-  /*
-   * Sum the Euclidean distance from the camera to every tag that contributed
-   * to this pose estimate.
-   */
-  for (var target : est.targetsUsed) {
-    sum += target.getBestCameraToTarget().getTranslation().getNorm();
-  }
-
-  // Return the mean distance across all contributing tags.
-  return sum / est.targetsUsed.size();
-}
-
-/**
- * Finds the highest pose ambiguity among all AprilTags used in a pose estimate.
- *
- * <p>Pose ambiguity is a PhotonVision-provided measure describing how uncertain a tag-based
- * pose solve is. A larger ambiguity generally indicates a less trustworthy estimate.
- * This method uses the worst contributing tag as a conservative quality metric.
- *
- * @param est the estimated robot pose containing the list of AprilTag targets used
- * @return the maximum ambiguity value across all used tags, or {@code 1.0} if no targets were used
- */
-private double maxAmbiguity(EstimatedRobotPose est) {
-  // If no tags were used, return a pessimistic ambiguity value.
-  if (est.targetsUsed == null || est.targetsUsed.isEmpty()) return 1.0;
-
-  double max = 0.0;
-
-  /*
-   * Track the worst ambiguity among all tags contributing to this estimate.
-   * Using the maximum ambiguity is intentionally conservative.
-   */
-  for (var target : est.targetsUsed) {
-    max = Math.max(max, target.getPoseAmbiguity());
-  }
-
-  return max;
-}
-
-/**
- * Determines whether two robot poses agree closely enough to be considered consistent.
- *
- * <p>Agreement is based on both translation and rotation:
- * <ul>
- *   <li>The XY distance between the poses must be less than {@code maxXY}</li>
- *   <li>The angular difference between the poses must be less than {@code maxThetaRad}</li>
- * </ul>
- *
- * @param posA the first pose
- * @param posB the second pose
- * @param maxXY the maximum allowed translational difference in meters
- * @param maxThetaRad the maximum allowed rotational difference in radians
- * @return {@code true} if the poses are within both tolerances, otherwise {@code false}
- */
-private boolean agrees(Pose2d posA, Pose2d posB, double maxXY, double maxThetaRad) {
-  return posA.getTranslation().getDistance(posB.getTranslation()) < maxXY
-      && Math.abs(posA.getRotation().minus(posB.getRotation()).getRadians()) < maxThetaRad;
-}
-
-/**
- * Filters a list of vision observations by requiring cross-camera agreement or strong multitag support.
- *
- * <p>An observation is accepted if either:
- * <ul>
- *   <li>At least one other camera reports a sufficiently similar pose, or</li>
- *   <li>The observation is a strong multitag estimate</li>
- * </ul>
- *
- * <p>This helps reject outliers from single-camera failures while still allowing good multitag
- * estimates to pass through even if no other camera currently agrees.
- *
- * @param obs the raw list of vision observations
- * @return a filtered list containing only observations that passed consensus checks
- */
-private List<VisionObservation> filterByConsensus(List<VisionObservation> obs) {
-  List<VisionObservation> accepted = new ArrayList<>();
-
-  /*
-   * Compare every observation against every other observation.
-   * If another camera agrees with it, it gains support and is more likely to be valid.
+  /**
+   * Update the pose estimation inside of {@link SwerveDrive} with all of the
+   * given poses.
+   *
+   * @param swerveDrive {@link SwerveDrive} instance.
    */
-  for (VisionObservation obsA : obs) {
-    int agreements = 0;
-
-    for (VisionObservation obsB : obs) {
-      // Skip comparing an observation to itself.
-      if (obsA == obsB) continue;
-
+  public void updatePoseEstimation(SwerveDrive swerveDrive) {
+    if (SwerveDriveTelemetry.isSimulation && swerveDrive.getSimulationDriveTrainPose().isPresent()) {
       /*
-       * Count how many other observations are spatially consistent with this one.
-       * The thresholds here define what "agreement" means across cameras.
+       * In the maple-sim, odometry is simulated using encoder values, accounting for
+       * factors like skidding and drifting.
+       * As a result, the odometry may not always be 100% accurate.
+       * However, the vision system should be able to provide a reasonably accurate
+       * pose estimation, even when odometry is incorrect.
+       * (This is why teams implement vision system to correct odometry.)
+       * Therefore, we must ensure that the actual robot pose is provided in the
+       * simulator when updating the vision simulation during the simulation.
        */
-      if (agrees(
-          obsA.pose,
-          obsB.pose,
-          CONSENSUS_MAX_TRANSLATION_METERS,
-          CONSENSUS_MAX_ROTATION_RADIANS)) {
-        agreements++;
-      }
-    }
-
-    /*
-     * Keep the observation if:
-     * 1. At least one other camera agrees with it, indicating cross-camera consensus, or
-     * 2. It is a strong multitag result, which is generally more trustworthy on its own.
-     */
-    if (agreements > 0
-        || (obsA.isMultiTag
-            && obsA.tagCount >= 2
-            && obsA.avgTagDistance < STRONG_MULTITAG_MAX_DISTANCE_METERS)) {
-      accepted.add(obsA);
+      visionSim.update(swerveDrive.getSimulationDriveTrainPose().get());
     }
-  }
-
-  return accepted;
-}
-
-/**
- * Determines whether a vision measurement should be accepted given the current robot state.
- *
- * <p>Robot motion and odometry updates are actively occurring, so blindly accepting large pose
- * jumps can destabilize localization. This method allows:
- * <ul>
- *   <li>Measurements close to the current pose estimate</li>
- *   <li>Larger corrections if the vision is especially strong</li>
- *   <li>Larger corrections if multiple observations support the measurement</li>
- * </ul>
- *
- * @param obs the vision observation being considered
- * @param currentPose the robot's current estimated pose from odometry / pose estimation
- * @param speeds the robot's current chassis speeds
- * @param supportCount the number of accepted observations supporting the current vision update cycle
- * @return {@code true} if the measurement should be applied, otherwise {@code false}
- */
-private boolean shouldAcceptEnabledMeasurement(
-    VisionObservation obs,
-    Pose2d currentPose,
-    ChassisSpeeds speeds,
-    int supportCount) {
-
-  // Compute translational and angular error between the current estimate and vision.
-  double translationalError =
-      currentPose.getTranslation().getDistance(obs.pose.getTranslation());
-  double headingError =
-      Math.abs(currentPose.getRotation().minus(obs.pose.getRotation()).getRadians());
-
-  // Use planar speed magnitude to scale allowable positional correction.
-  double speed = Math.hypot(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond);
-
-  /*
-   * Base tolerance:
-   * - Allow larger translational jumps when the robot is moving faster, since odometry can drift
-   *   more under motion.
-   * - Keep heading jump tolerance fixed for now.
-   */
-  double maxTranslationJump = 0.5 + 0.35 * speed;
-  double maxHeadingJump = Math.toRadians(20);
-
-  /*
-   * Define conditions under which a larger-than-normal correction is acceptable:
-   * - strongVision: multitag solution with reasonably close tags
-   * - hasSupport: multiple accepted observations this cycle
-   */
-  boolean strongVision = obs.isMultiTag && obs.avgTagDistance < 4.0;
-  boolean hasSupport = supportCount >= 2;
-
-  // Accept small, ordinary corrections immediately.
-  if (translationalError < maxTranslationJump && headingError < maxHeadingJump) {
-    return true;
-  }
-
-  /*
-   * Otherwise, only accept the correction if the vision data is especially trustworthy
-   * or multiple observations support it.
-   */
-  return strongVision || hasSupport;
-}
-
-/**
- * Updates the robot pose estimator with valid vision measurements from all configured cameras.
- *
- * <p>This method performs the following high-level steps:
- * <ol>
- *   <li>Updates the vision simulator during simulation</li>
- *   <li>Collects pose estimates from each camera</li>
- *   <li>Builds {@link VisionObservation} objects with quality metadata</li>
- *   <li>Filters out inconsistent observations using cross-camera consensus</li>
- *   <li>Falls back to multitag-only observations if consensus fails</li>
- *   <li>Applies accepted measurements to the swerve drive pose estimator</li>
- * </ol>
- *
- * <p>The goal is to make vision updates robust against bad individual camera measurements while
- * still allowing strong observations to correct odometry drift.
- *
- * @param swerveDrive the {@link SwerveDrive} instance whose pose estimator should be updated
- */
-public void updatePoseEstimation(SwerveDrive swerveDrive) {
-  if (SwerveDriveTelemetry.isSimulation && swerveDrive.getSimulationDriveTrainPose().isPresent()) {
-    /*
-     * In simulation, the drivetrain pose used by the simulator represents the "ground truth"
-     * position of the robot.
-     *
-     * Simulated odometry may still include effects such as skidding or drift, meaning the pose
-     * estimator's internal odometry can deviate from truth just like on a real robot.
-     *
-     * Since vision exists specifically to correct odometry drift, the vision simulator should
-     * be updated using the true simulated robot pose rather than the potentially drifted estimate.
-     */
-    visionSim.update(swerveDrive.getSimulationDriveTrainPose().get());
-  }
-
-  // Read the robot's current estimated pose and velocity for gating future measurements.
-  Pose2d currentPose = swerveDrive.getPose();
-  ChassisSpeeds speeds = swerveDrive.getRobotVelocity();
-
-  List<VisionObservation> observations = new ArrayList<>();
-
-  /*
-   * Query every configured camera for a global pose estimate.
-   * Each valid estimate is converted into a VisionObservation containing
-   * pose data plus metadata used for filtering and trust evaluation.
-   */
-  for (Cameras camera : Cameras.values()) {
-    Optional<EstimatedRobotPose> poseEst = getEstimatedGlobalPose(camera);
-
-    // Skip cameras that do not currently have a valid estimate.
-    if (!poseEst.isPresent()) continue;
-
-    EstimatedRobotPose estPos = poseEst.get();
-    Pose2d estPose2d = estPos.estimatedPose.toPose2d();
-
-    // Determine how many tags contributed to this estimate.
-    int tagsUsed = estPos.targetsUsed == null ? 0 : estPos.targetsUsed.size();
-
-    // Ignore estimates that were not built from any tags.
-    if (tagsUsed == 0) continue;
-
-    // Compute quality metrics used later for filtering and acceptance decisions.
-    double avgDist = averageTagDistance(estPos);
-    double maxTagAmbiguity = maxAmbiguity(estPos);
-    boolean isMultiTag = tagsUsed >= 2;
-    Matrix<N3, N1> currStdDevs = camera.curStdDevs;
-
-    // Reject highly ambiguous estimates before they enter the consensus pipeline.
-    if (maxTagAmbiguity > maximumAmbiguity) {
-      continue;
-    }
-
-    /*
-     * Store a normalized observation record for later filtering.
-     * This keeps the later pipeline cleaner by collecting all relevant
-     * metadata in one place.
-     */
-    observations.add(new VisionObservation(
-      camera,
-      estPos,
-      estPose2d,
-      estPos.timestampSeconds,
-      tagsUsed,
-      avgDist,
-      maxTagAmbiguity,
-      isMultiTag,
-      currStdDevs
-    ));
-  }
-
-  // Nothing to do if no camera produced a usable observation.
-  if (observations.isEmpty()) return;
-
-  // First-pass rejection of outliers using cross-camera agreement.
-  List<VisionObservation> filtered = filterByConsensus(observations);
-
-  if (filtered.isEmpty()) {
-    /*
-     * If no observations achieved consensus, fall back to multitag-only observations.
-     * This gives strong single-camera multitag estimates a chance to still correct pose.
-     */
-    filtered = observations.stream()
-        .filter(o -> o.isMultiTag)
-        .toList();
-  }
-
-  // If nothing survived filtering or fallback, skip this update cycle entirely.
-  if (filtered.isEmpty()) {
-    return;
-  }
-
-  /*
-   * Compute per-observation support: for each observation, count how many other
-   * observations have a sufficiently similar pose (i.e., they agree). This avoids
-   * treating all multitag observations as mutually supporting when they disagree.
-   */
-  int n = filtered.size();
-  List<Integer> supportCounts = new ArrayList<>(n);
-  for (int i = 0; i < n; i++) {
-    // Start each observation with a support count of 1 (itself).
-    supportCounts.add(1);
-  }
-
-  // Thresholds for considering two observations to be in agreement.
-  final double translationEpsMeters = 0.5; // positional agreement threshold
-  final double rotationEpsRadians = Math.toRadians(10.0); // angular agreement threshold
-
-  for (int i = 0; i < n; i++) {
-    Pose2d poseI = filtered.get(i).pose;
-    for (int j = i + 1; j < n; j++) {
-      Pose2d poseJ = filtered.get(j).pose;
-      Transform2d delta = poseI.minus(poseJ);
-      double dist = delta.getTranslation().getNorm();
-      double rotDiff = Math.abs(delta.getRotation().getRadians());
-      if (dist <= translationEpsMeters && rotDiff <= rotationEpsRadians) {
-        // Observations i and j agree; increment support for both.
-        supportCounts.set(i, supportCounts.get(i) + 1);
-        supportCounts.set(j, supportCounts.get(j) + 1);
+    for (Cameras camera : Cameras.values()) {
+      Optional<EstimatedRobotPose> poseEst = getEstimatedGlobalPose(camera);
+      if (poseEst.isPresent()) {
+
+        var pose = poseEst.get();
+        swerveDrive.addVisionMeasurement(
+            pose.estimatedPose.toPose2d(),
+            pose.timestampSeconds,
+            camera.curStdDevs);
+
+        SmartDashboard.putNumber("VisionX", pose.estimatedPose.getX());
+        SmartDashboard.putNumber("VisionY", pose.estimatedPose.getY());
       }
     }
-  }
 
-  /*
-   * Feed accepted vision measurements into the drivetrain pose estimator.
-   * Each measurement is individually gated against current pose and robot motion.
-   */
-  for (int i = 0; i < n; i++) {
-    VisionObservation obs = filtered.get(i);
-    int support = supportCounts.get(i);
-    if (shouldAcceptEnabledMeasurement(obs, currentPose, speeds, support)) {
-      swerveDrive.addVisionMeasurement(obs.pose, obs.timestamp, obs.stdDevs);
-    }
   }
-}
 
   /**
    * Generates the estimated robot pose. Returns empty if:
diff --git a/src/main/java/frc/robot/util/vision/VisionObservation.java b/src/main/java/frc/robot/util/vision/VisionObservation.java
deleted file mode 100644
index bcac2d9..0000000
--- a/src/main/java/frc/robot/util/vision/VisionObservation.java
+++ /dev/null
@@ -1,41 +0,0 @@
-package frc.robot.util.vision;
-
-import org.photonvision.EstimatedRobotPose;
-
-import edu.wpi.first.math.Matrix;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.numbers.N1;
-import edu.wpi.first.math.numbers.N3;
-
-public class VisionObservation {
-  public final Cameras camera;
-  public final EstimatedRobotPose estimate;
-  public final Pose2d pose;
-  public final double timestamp;
-  public final int tagCount;
-  public final double avgTagDistance;
-  public final double ambiguity;
-  public final boolean isMultiTag;
-  public final Matrix<N3, N1> stdDevs;
-
-  public VisionObservation(
-      Cameras camera,
-      EstimatedRobotPose estimate,
-      Pose2d pose,
-      double timestamp,
-      int tagCount,
-      double avgTagDistance,
-      double ambiguity,
-      boolean isMultiTag,
-      Matrix<N3, N1> stdDevs) {
-    this.camera = camera;
-    this.estimate = estimate;
-    this.pose = pose;
-    this.timestamp = timestamp;
-    this.tagCount = tagCount;
-    this.avgTagDistance = avgTagDistance;
-    this.ambiguity = ambiguity;
-    this.isMultiTag = isMultiTag;
-    this.stdDevs = stdDevs;
-  }
-}
\ No newline at end of file

From 4ade3fa3fee9bf2de821da7c0cd7897f13aa7f6f Mon Sep 17 00:00:00 2001
From: N/A <cosmicaura3@gmail.com>
Date: Sun, 29 Mar 2026 13:08:00 -0500
Subject: [PATCH 22/26] feat: add new auto options

---
 src/main/java/frc/robot/Constants.java        | 23 ++++++++++++++++
 src/main/java/frc/robot/RobotContainer.java   | 26 ++++++++++++++++---
 .../frc/robot/util/auto/IntakeStrategy.java   |  4 +++
 3 files changed, 49 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 5507c65..cf1a81b 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -98,6 +98,19 @@ public static class Auto {
                 new Pose2d(7.730, 6.457, Rotation2d.fromDegrees(-160))
             );
 
+            public static final List<Pose2d> BACK_N_FORTH_RIGHT_SEQUENCE = List.of(
+                new Pose2d(8.500, 2.1, Rotation2d.fromDegrees(-160)),
+                new Pose2d(8.306, 6.707, Rotation2d.fromDegrees(-160)),
+                new Pose2d(8.154, 1.349, Rotation2d.fromDegrees(-160))
+            );
+
+            
+            public static final List<Pose2d> BACK_N_FORTH_LEFT_SEQUENCE = List.of(
+                new Pose2d(8.500, 6.000, Rotation2d.fromDegrees(-160)),
+                new Pose2d(8.154, 1.349, Rotation2d.fromDegrees(-160)),
+                new Pose2d(8.306, 6.707, Rotation2d.fromDegrees(-160))
+            );
+
             public static final Translation2d NEUTRAL_RIGHT_SPIN_TRANSLATION = new Translation2d(
                 7.730,
                 7.457
@@ -120,6 +133,16 @@ public static class Auto {
                 new Pose2d(7.829,4.051,Rotation2d.kCW_90deg)
             );
 
+            public static final List<Pose2d> RAM_SS_LEFT_SEQUENCE = List.of(
+                new Pose2d(7.797,5.902,Rotation2d.kCW_90deg),
+                new Pose2d(7.829,4.400,Rotation2d.kCW_90deg)
+            );
+
+            public static final List<Pose2d> RAM_SS_RIGHT_SEQUENCE = List.of(
+                new Pose2d(7.797,2.168,Rotation2d.kCW_90deg),
+                new Pose2d(7.829,3.650,Rotation2d.kCW_90deg)
+            );
+
 
         }
 
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 7324569..63b5fb3 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -281,10 +281,10 @@ public void configureBindings() {
 
     Trigger intakeTrigger = new Trigger(() -> driverController.getRightBumperButton());
 
-    Trigger reverseTransferTrigger = new Trigger(() -> driverController.getXButton());
+    //Trigger reverseTransferTrigger = new Trigger(() -> driverController.getXButton());
     
     // Driver X button: hold to lock robot pose (X-lock)
-    Trigger xLockTrigger = new Trigger(() -> operatorController.getXButton());
+    Trigger xLockTrigger = new Trigger(() -> driverController.getXButton());
     xLockTrigger.whileTrue(driveSubsystem.getLockPoseCommand());
 
     /* Shooter runs while button held */
@@ -321,8 +321,8 @@ public void configureBindings() {
             intakePivot.raiseIntakeCommand(),
             intakeRoller.getStopCommand()));
 
-    reverseTransferTrigger.whileTrue(
-      transferSubsystem.getSetPowerCommand(-1)).onFalse(transferSubsystem.getSetPowerCommand(0));
+    // reverseTransferTrigger.whileTrue(
+    //   transferSubsystem.getSetPowerCommand(-1)).onFalse(transferSubsystem.getSetPowerCommand(0));
 
     // =========================
     // Turret Offset Adjustment (POV Left / Right)
@@ -682,6 +682,16 @@ private Command getIntakeStrategyCommand() {
             .map(FieldUtil::flipIfRed)
             .toList());
 
+      case RAM_SS_L:
+        return new DriveToSequenceCommand(driveSubsystem, Constants.Operator.Auto.RAM_SS_LEFT_SEQUENCE.stream()
+            .map(FieldUtil::flipIfRed)
+            .toList());
+
+      case RAM_SS_R:
+        return new DriveToSequenceCommand(driveSubsystem, Constants.Operator.Auto.RAM_SS_RIGHT_SEQUENCE.stream()
+            .map(FieldUtil::flipIfRed)
+            .toList());
+
       case CUSTOM:
 
         Pose2d ready = getDashboardPose("Auto/CustomReadyPose");
@@ -704,6 +714,14 @@ private Command getIntakeStrategyCommand() {
             Constants.Operator.Auto.AUTO_INTAKE_MAX_SPEED,
             SmartDashboard.getNumber("Auto/IntakeShootTime", Constants.Operator.Auto.DEFAULT_INTAKE_SHOOT_TIME));
 
+      case B_N_F_R:
+        return new DriveToSequenceCommand(driveSubsystem, Constants.Operator.Auto.BACK_N_FORTH_RIGHT_SEQUENCE.stream()
+              .map(FieldUtil::flipIfRed)
+              .toList());
+        case B_N_F_L:
+        return new DriveToSequenceCommand(driveSubsystem, Constants.Operator.Auto.BACK_N_FORTH_LEFT_SEQUENCE.stream()
+              .map(FieldUtil::flipIfRed)
+              .toList());
       default:
         return Commands.none();
     }
diff --git a/src/main/java/frc/robot/util/auto/IntakeStrategy.java b/src/main/java/frc/robot/util/auto/IntakeStrategy.java
index 5a9fab8..d900b71 100644
--- a/src/main/java/frc/robot/util/auto/IntakeStrategy.java
+++ b/src/main/java/frc/robot/util/auto/IntakeStrategy.java
@@ -9,6 +9,10 @@ public enum IntakeStrategy {
     RAM_LEFT,
     RAM_RIGHT,
     JUST_SHOOT,
+    B_N_F_L,
+    B_N_F_R,
+    RAM_SS_L,
+    RAM_SS_R,
     CUSTOM
     
 }

From fe5cc4a7043059a9d95c02c515c4185b78e1df2f Mon Sep 17 00:00:00 2001
From: N/A <cosmicaura3@gmail.com>
Date: Tue, 7 Apr 2026 18:22:14 -0500
Subject: [PATCH 23/26] feat: update target setpoints

---
 src/main/java/frc/robot/Constants.java | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index cf1a81b..b04c2cb 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -99,16 +99,16 @@ public static class Auto {
             );
 
             public static final List<Pose2d> BACK_N_FORTH_RIGHT_SEQUENCE = List.of(
-                new Pose2d(8.500, 2.1, Rotation2d.fromDegrees(-160)),
-                new Pose2d(8.306, 6.707, Rotation2d.fromDegrees(-160)),
-                new Pose2d(8.154, 1.349, Rotation2d.fromDegrees(-160))
+                new Pose2d(8.500, 2.1, Rotation2d.fromDegrees(-45)),
+                new Pose2d(8.306, 6.707, Rotation2d.fromDegrees(-45)),
+                new Pose2d(8.154, 1.349, Rotation2d.fromDegrees(-45))
             );
 
             
             public static final List<Pose2d> BACK_N_FORTH_LEFT_SEQUENCE = List.of(
-                new Pose2d(8.500, 6.000, Rotation2d.fromDegrees(-160)),
-                new Pose2d(8.154, 1.349, Rotation2d.fromDegrees(-160)),
-                new Pose2d(8.306, 6.707, Rotation2d.fromDegrees(-160))
+                new Pose2d(8.500, 6.000, Rotation2d.fromDegrees(-45)),
+                new Pose2d(8.154, 1.349, Rotation2d.fromDegrees(-45)),
+                new Pose2d(8.306, 6.707, Rotation2d.fromDegrees(-45))
             );
 
             public static final Translation2d NEUTRAL_RIGHT_SPIN_TRANSLATION = new Translation2d(

From 98e280422b4fb8d8e666bae5d51ab6ab6f9f6edb Mon Sep 17 00:00:00 2001
From: Cameron Myhre <cameronmyhre@icloud.com>
Date: Tue, 7 Apr 2026 18:26:35 -0500
Subject: [PATCH 24/26] Update src/main/java/frc/robot/RobotContainer.java

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>
---
 src/main/java/frc/robot/RobotContainer.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 63b5fb3..93305b1 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -718,7 +718,7 @@ private Command getIntakeStrategyCommand() {
         return new DriveToSequenceCommand(driveSubsystem, Constants.Operator.Auto.BACK_N_FORTH_RIGHT_SEQUENCE.stream()
               .map(FieldUtil::flipIfRed)
               .toList());
-        case B_N_F_L:
+      case B_N_F_L:
         return new DriveToSequenceCommand(driveSubsystem, Constants.Operator.Auto.BACK_N_FORTH_LEFT_SEQUENCE.stream()
               .map(FieldUtil::flipIfRed)
               .toList());

From c66f4e861a2da32eb912f3ee166b4b779ef76e48 Mon Sep 17 00:00:00 2001
From: N/A <cosmicaura3@gmail.com>
Date: Tue, 7 Apr 2026 18:30:25 -0500
Subject: [PATCH 25/26] feat: uncomment removed, tested functionality

---
 src/main/java/frc/robot/RobotContainer.java | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 93305b1..8f9280c 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -281,7 +281,7 @@ public void configureBindings() {
 
     Trigger intakeTrigger = new Trigger(() -> driverController.getRightBumperButton());
 
-    //Trigger reverseTransferTrigger = new Trigger(() -> driverController.getXButton());
+    Trigger reverseTransferTrigger = new Trigger(() -> driverController.getXButton());
     
     // Driver X button: hold to lock robot pose (X-lock)
     Trigger xLockTrigger = new Trigger(() -> driverController.getXButton());
@@ -321,8 +321,8 @@ public void configureBindings() {
             intakePivot.raiseIntakeCommand(),
             intakeRoller.getStopCommand()));
 
-    // reverseTransferTrigger.whileTrue(
-    //   transferSubsystem.getSetPowerCommand(-1)).onFalse(transferSubsystem.getSetPowerCommand(0));
+    reverseTransferTrigger.whileTrue(
+       transferSubsystem.getSetPowerCommand(-1)).onFalse(transferSubsystem.getSetPowerCommand(0));
 
     // =========================
     // Turret Offset Adjustment (POV Left / Right)

From fd9f6694d56ccc0f3f7a8151e100da3779bbef35 Mon Sep 17 00:00:00 2001
From: Cameron Myhre <cameronmyhre@icloud.com>
Date: Tue, 7 Apr 2026 18:39:26 -0500
Subject: [PATCH 26/26] Update
 src/main/java/frc/robot/commands/ShootPreloadCommand.java

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>
---
 src/main/java/frc/robot/commands/ShootPreloadCommand.java | 8 +++++---
 1 file changed, 5 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc/robot/commands/ShootPreloadCommand.java b/src/main/java/frc/robot/commands/ShootPreloadCommand.java
index 2207b2c..ac24416 100644
--- a/src/main/java/frc/robot/commands/ShootPreloadCommand.java
+++ b/src/main/java/frc/robot/commands/ShootPreloadCommand.java
@@ -33,15 +33,17 @@ public ShootPreloadCommand(
 
             Commands.deadline(
                 Commands.waitSeconds(shootPreloadTime + 2),
-                new SequentialCommandGroup(
+                Commands.parallel(
                     new ParallelCommandGroup(
                         turret.getTargetCommand(targetSupplier, robotPoseSupplier, robotVelocitySupplier),
                         shooter.getTargetCommand(targetSupplier, robotPoseSupplier, robotVelocitySupplier),
                         intakeRoller.getIntakeCommand(),
                         intakePivot.deployIntakeCommand()
                     ),
-                    new WaitCommand(2),
-                    transfer.getLoadCommand()
+                    Commands.sequence(
+                        new WaitCommand(2),
+                        transfer.getLoadCommand()
+                    )
                 )
             ),