Class Vision

java.lang.Object
frc.robot.subsystems.vision.Vision
All Implemented Interfaces:
Subsystem
public class Vision extends Object implements Subsystem
FM's multi-source localization testbed: up to three Limelights, three Orin/PhotonVision cameras and a QuestNav, each gated on its own and each logged well enough to compare after the fact.

Every loop

  1. Every source reads its device into logged inputs (PoseSource.update()), whether or not it is enabled.
  2. Every source gates every observation and logs the verdict and reason.
  3. PoseFusion applies each source's accepted measurements to that source's shadow track, and to the robot's fused pose if the source is enabled and its fusion policy allows it right now (see below).
  4. While disabled, the best Limelight's MegaTag1 seeds the heading and translation of every track. While enabled, the gross-heading safety net can re-seed once a camera has disagreed with the gyro badly for a full second.

Fusion policy (from the 2026 code)

  • Limelights fuse while enabled only in teleop, or in auto while the path asks for pose updates or the robot is launching (FM's main).
  • Every chassis Limelight fuses, not just the best one (offseason: the estimator already weights each by its std-devs).
  • MegaTag1 until the disabled seed is confirmed, MegaTag2 after (offseason, switchable at Vision/ChassisUseMT2).
  • The Orin and QuestNav fuse whenever enabled and the robot is enabled. Both start disabled: they are logged and shadowed, but do not move the robot pose until someone flips /Localization/Sources/<name>/Enabled on the dashboard.

Not ported from the offseason branch: the placement-heading vote and the two-camera consensus heading correction (both need the offseason bot's camera geometry to be meaningful), and everything turret-camera specific.

  • Constructor Details

  • Method Details

    • getName

      public String getName()
      Specified by:
      getName in interface Subsystem

    • periodic

      public void periodic()
      Specified by:
      periodic in interface Subsystem

    • isChassisUseMt2

      public boolean isChassisUseMt2()
      Whether the dashboard switch to fuse MegaTag2 once the seed is confirmed is on.

    • resetQuestToRobotPose

      public void resetQuestToRobotPose()
      Sends the Quest the current fused pose.

    • secondsSinceFusedEstimate

      public double secondsSinceFusedEstimate()
      Seconds since a vision measurement last moved the fused pose; infinite before the first one. How long the pose has been running on odometry alone. From logged inputs and the replayed clock, so logic may read it.

    • hasAccuratePose

      public boolean hasAccuratePose()
      Whether any Limelight produced an accepted estimate this loop.

    • triggerRewindCaptureForAllCameras

      public void triggerRewindCaptureForAllCameras(double seconds)
      Saves the last seconds of the rewind buffer of each FM Limelight: video plus every frame's results, reviewable frame by frame afterwards. Rewind is a Limelight 4 feature; the camera writes the capture to its own storage, which survives a power cycle (checked 2026-10-16), so the robot can be powered off after a match.

      Not the SystemCore's own cameras: their "camera storage" is the robot controller's internal disk, which LogStorage is keeping free.

      Parameters:
      seconds - how much to save, at most 165

    • setLimelightPipelines

      public void setLimelightPipelines(int pipeline)
      Sets all Limelights to a pipeline.

    • blinkLimelights

      public Command blinkLimelights()
      Blinks every Limelight's LEDs while held.

    • resetQuestCommand

      public Command resetQuestCommand()
      Re-origins the QuestNav onto the current robot pose.

    • seedConfirmed

      public BooleanSupplier seedConfirmed()
      A BooleanSupplier for whether the pose seed is confirmed.

    • getTagField

      public static AprilTagFields getTagField()
      The field the robot solves with: this event's (Event.APRILTAG_FIELD), or in replay the match's (see TagField). The Jetson's must match.

    • getTagLayout

      public static AprilTagFieldLayout getTagLayout()

    • getTagLayoutHash

      public static String getTagLayoutHash()
      tagLayout's canonical hash (FieldMapHash).

    • getConfig

      public Vision.VisionConfig getConfig()

    • getCameraHeadings

      public CameraHeadingCheck getCameraHeadings()
      Pre-match: every tag-solving camera's heading against the gyro and each other.

    • getLimelights

      public List<Limelight> getLimelights()

    • getMt1Sources

      public List<PoseSource> getMt1Sources()

    • getMt2Sources

      public List<PoseSource> getMt2Sources()

    • getScMt1Sources

      public List<PoseSource> getScMt1Sources()

    • getScMt2Sources

      public List<PoseSource> getScMt2Sources()

    • getOrinSources

      public List<PoseSource> getOrinSources()

    • getOrinGyroSources

      public List<PoseSource> getOrinGyroSources()
      Orin sources built on the main ones: gyro-held PnP and tx/ty (issue #10, 3 and 8c).

    • getJetson

      public Jetson getJetson()

    • getQuestSource

      public PoseSource getQuestSource()

    • getAllSources

      public List<PoseSource> getAllSources()

    • isPoseHeadingSeeded

      public boolean isPoseHeadingSeeded()

    • isPoseSeedConfirmed

      public boolean isPoseSeedConfirmed()

    • getMultiTagSeedCount

      public long getMultiTagSeedCount()
      Times any track was seeded from a multi-tag MegaTag1 solve since boot. The pre-match start check compares it across a placement to know the pose came from a camera, not the code.