Add a pose buffer for vision measurements

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
    *