Add a pose buffer for vision measurements (#136)

* Add vision examples from good teams
* WIP: Getting 254's vision code to compile
* FRC254's 2024 vision code builds! -- Next step is to mesh in the parts of the vision code I need for the circular vision buffer to the base vision subsystem.
* WIP: Adding SPAM's vision code
* SPAM / FRC180 vision now builds -- The vision subsystem from FRC180 / SPAM now builds within the 2486 code base.  Next steps are to figure out which parts of the various vision combination schemes I want to use and hack together something!
* Add 6328's 2025 drive-to-pose algorithm
* Make cleaner diff against Az-RBSI
* Update FRC254's vision to 2025 version
* Fix rebase error
* Clean non-ASCII characters
* Compilable Vision with pose buffering -- Create a pose buffer and use timestamps to match vision values with those from odometry to reduce jitter.  Also be able to define certain tags in whose location we trust more (e.g. HUB in 2026) in the event of field misshapenness.  Weave the new Vision structure together with the drivetrain to allow for timestamped pose insertion into the estimator.

---------

Co-authored-by: NutHouse coco-prog-3 <github@team2486.org>

Author: tbowers7

src/main/java/frc/robot/Constants.java

@@ -416,6 +416,16 @@ public static final class AutoConstants {
   /** Vision Constants (Assuming PhotonVision) ***************************** */
   public static class VisionConstants {
 
+    public static final java.util.Set<Integer> kTrustedTags =
+        java.util.Set.of(2, 3, 4, 5, 8, 9, 10, 11, 18, 19, 20, 21, 24, 25, 26, 27); // HUB AprilTags
+
+    // Noise scaling factors (lower = more trusted)
+    public static final double kTrustedTagStdDevScale = 0.6; // 40% more weight
+    public static final double kUntrustedTagStdDevScale = 1.3; // 30% less weight
+
+    // Optional: if true, reject observations that contain no trusted tags
+    public static final boolean kRequireTrustedTag = false;
+
     // AprilTag Identification Constants
     public static final double kAmbiguityThreshold = 0.4;
     public static final double kTargetLogTimeSecs = 0.1;

src/main/java/frc/robot/Robot.java

@@ -19,6 +19,7 @@
 
 import com.revrobotics.util.StatusLogger;
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.Threads;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -47,6 +48,7 @@ public class Robot extends LoggedRobot {
   private Command m_autoCommandPathPlanner;
   private RobotContainer m_robotContainer;
   private Timer m_disabledTimer;
+  private static boolean isBlueAlliance = false;
 
   // Define simulation fields here
   private VisionSystemSim visionSim;
@@ -135,9 +137,13 @@ public Robot() {
   /** TESTING VERSION OF ROBOTPERIODIC FOR OVERRUN SOURCES */
   @Override
   public void robotPeriodic() {
+
+    isBlueAlliance = DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue;
+
     final long t0 = System.nanoTime();
 
     if (isReal()) {
+      // Switch thread to high priority to improve loop timing
       Threads.setCurrentThreadPriority(true, 99);
     }
     final long t1 = System.nanoTime();
@@ -186,6 +192,7 @@ public void autonomousInit() {
     CommandScheduler.getInstance().cancelAll();
     m_robotContainer.getDrivebase().setMotorBrake(true);
     m_robotContainer.getDrivebase().resetHeadingController();
+    m_robotContainer.getVision().resetPoseGate(Timer.getFPGATimestamp());
 
     // TODO: Make sure Gyro inits here with whatever is in the path planning thingie
     switch (Constants.getAutoType()) {
@@ -315,4 +322,13 @@ public void simulationPeriodic() {
     // Update sim each sim tick
     visionSim.update(m_robotContainer.getDrivebase().getPose());
   }
+
+  // Helper method to simplify checking if the robot is blue or red alliance
+  public static boolean isBlue() {
+    return isBlueAlliance;
+  }
+
+  public static boolean isRed() {
+    return !isBlue();
+  }
 }

src/main/java/frc/robot/RobotContainer.java

@@ -34,7 +34,6 @@
 import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.Constants.CANBuses;
-import frc.robot.Constants.Cameras;
 import frc.robot.Constants.OperatorConstants;
 import frc.robot.FieldConstants.AprilTagLayoutType;
 import frc.robot.commands.AutopilotCommands;
@@ -75,6 +74,9 @@
 /** This is the location for defining robot hardware, commands, and controller button bindings. */
 public class RobotContainer {
 
+  private static final boolean USE_MAPLESIM = true;
+  public static final boolean MAPLESIM = USE_MAPLESIM && Robot.isSimulation();
+
   /** Define the Driver and, optionally, the Operator/Co-Driver Controllers */
   // Replace with ``CommandPS4Controller`` or ``CommandJoystick`` if needed
   final CommandXboxController driverController = new CommandXboxController(0); // Main Driver
@@ -128,36 +130,7 @@ public class RobotContainer {
   // Alerts
   private final Alert aprilTagLayoutAlert = new Alert("", AlertType.INFO);
 
-  // Vision Factories
-  private VisionIO[] buildVisionIOsReal(Drive drive) {
-    return switch (Constants.getVisionType()) {
-      case PHOTON ->
-          Arrays.stream(Cameras.ALL)
-              .map(c -> (VisionIO) new VisionIOPhotonVision(c.name(), c.robotToCamera()))
-              .toArray(VisionIO[]::new);
-
-      case LIMELIGHT ->
-          Arrays.stream(Cameras.ALL)
-              .map(c -> (VisionIO) new VisionIOLimelight(c.name(), drive::getHeading))
-              .toArray(VisionIO[]::new);
-
-      case NONE -> new VisionIO[] {new VisionIO() {}};
-    };
-  }
-
-  private static VisionIO[] buildVisionIOsSim(Drive drive) {
-    var cams = Constants.Cameras.ALL;
-    VisionIO[] ios = new VisionIO[cams.length];
-    for (int i = 0; i < cams.length; i++) {
-      var cfg = cams[i];
-      ios[i] = new VisionIOPhotonVisionSim(cfg.name(), cfg.robotToCamera(), drive::getPose);
-    }
-    return ios;
-  }
-
-  private VisionIO[] buildVisionIOsReplay() {
-    return new VisionIO[] {new VisionIO() {}};
-  }
+  public static RobotContainer instance;
 
   /**
    * Constructor for the Robot Container. This container holds subsystems, opertator interface
@@ -179,7 +152,9 @@ public RobotContainer() {
 
         m_drivebase = new Drive(m_imu);
         m_flywheel = new Flywheel(new FlywheelIOSim()); // new Flywheel(new FlywheelIOTalonFX());
-        m_vision = new Vision(m_drivebase::addVisionMeasurement, buildVisionIOsReal(m_drivebase));
+        m_vision =
+            new Vision(
+                m_drivebase, m_drivebase::addVisionMeasurement, buildVisionIOsReal(m_drivebase));
         m_accel = new Accelerometer(m_imu);
         sweep = null;
         break;
@@ -193,11 +168,9 @@ public RobotContainer() {
 
         // ---------------- Vision IOs (robot code) ----------------
         var cams = frc.robot.Constants.Cameras.ALL;
-
-        // If you keep Vision expecting exactly two cameras:
-        VisionIO[] visionIOs = buildVisionIOsSim(m_drivebase);
-        m_vision = new Vision(m_drivebase::addVisionMeasurement, visionIOs);
-
+        m_vision =
+            new Vision(
+                m_drivebase, m_drivebase::addVisionMeasurement, buildVisionIOsSim(m_drivebase));
         m_accel = new Accelerometer(m_imu);
 
         // ---------------- CameraSweepEvaluator (sim-only analysis) ----------------
@@ -231,7 +204,8 @@ public RobotContainer() {
         m_imu = new Imu(new ImuIOSim() {});
         m_drivebase = new Drive(m_imu);
         m_flywheel = new Flywheel(new FlywheelIO() {});
-        m_vision = new Vision(m_drivebase::addVisionMeasurement, buildVisionIOsReplay());
+        m_vision =
+            new Vision(m_drivebase, m_drivebase::addVisionMeasurement, buildVisionIOsReplay());
         m_accel = new Accelerometer(m_imu);
         sweep = null;
         break;
@@ -493,6 +467,11 @@ public Drive getDrivebase() {
     return m_drivebase;
   }
 
+  /** Vision getter method for use with Robot.java */
+  public Vision getVision() {
+    return m_vision;
+  }
+
   /**
    * Set up the SysID routines from AdvantageKit
    *
@@ -536,6 +515,40 @@ private void definesysIdRoutines() {
     }
   }
 
+  // Vision Factories
+  // Vision Factories (REAL)
+  private VisionIO[] buildVisionIOsReal(Drive drive) {
+    return switch (Constants.getVisionType()) {
+      case PHOTON ->
+          java.util.Arrays.stream(Constants.Cameras.ALL)
+              .map(c -> (VisionIO) new VisionIOPhotonVision(c.name(), c.robotToCamera()))
+              .toArray(VisionIO[]::new);
+
+      case LIMELIGHT ->
+          java.util.Arrays.stream(Constants.Cameras.ALL)
+              .map(c -> (VisionIO) new VisionIOLimelight(c.name(), drive::getHeading))
+              .toArray(VisionIO[]::new);
+
+      case NONE -> new VisionIO[] {}; // recommended: no cameras
+    };
+  }
+
+  // Vision Factories (SIM)
+  private VisionIO[] buildVisionIOsSim(Drive drive) {
+    var cams = Constants.Cameras.ALL;
+    VisionIO[] ios = new VisionIO[cams.length];
+    for (int i = 0; i < cams.length; i++) {
+      var cfg = cams[i];
+      ios[i] = new VisionIOPhotonVisionSim(cfg.name(), cfg.robotToCamera(), drive::getPose);
+    }
+    return ios;
+  }
+
+  // Vision Factories (REPLAY)
+  private VisionIO[] buildVisionIOsReplay() {
+    return new VisionIO[] {}; // simplest: Vision does nothing during replay
+  }
+
   /**
    * Example Choreo auto command
    *