Package frc.robot.subsystems.vision

Class Vision

java.lang.Object
frc.robot.subsystems.vision.Vision
All Implemented Interfaces:
Subsystem
public class Vision extends Object implements Subsystem
Vision subsystem that manages three Limelights (back, left, right) and fuses their pose estimates into the swerve odometry via WPILib's SwerveDrivePoseEstimator.

Each robot loop iteration the subsystem:

  1. Pushes the robot's current heading to all Limelights so MegaTag2 IMU fusion remains accurate.
  2. Selects the Limelight with the best view (getBestLimelight()).
  3. Runs the MT1 (and, while disabled, MT2) rejection pipeline and adds accepted estimates to the pose estimator with appropriate std-dev vectors.
  4. Logs per-camera status and pose data via VisionLogger.

  • Field Details

    • backLL

      public final Limelight backLL
      Rear-facing Limelight instance.

    • leftLL

      public final Limelight leftLL
      Left-facing Limelight instance.

    • rightLL

      public final Limelight rightLL
      Right-facing Limelight instance.

    • allLimelights

      public final Limelight[] allLimelights
      All three Limelights in one array for bulk operations.

  • Constructor Details

    • Vision

      public Vision(Vision.VisionConfig config)
      Creates the Vision subsystem.

      Instantiates all three Limelights, registers their loggers, applies initial camera settings (LEDs off, IMU mode 1), loads the AprilTag field layout, and registers this subsystem with the WPILib scheduler.

      Parameters:
      config - the static configuration object

  • Method Details

    • getName

      public String getName()
      Specified by:
      getName in interface Subsystem
      Returns:
      the subsystem name defined in Vision.VisionConfig.

    • periodic

      public void periodic()
      Called every robot loop iteration by the WPILib scheduler.
      1. Pushes current heading to all Limelights (required for MegaTag2).
      2. Runs pose-estimation updates appropriate to the current robot mode.
      3. Logs telemetry for all cameras.
      Specified by:
      periodic in interface Subsystem

    • logTelemetry

      public void logTelemetry()
      Logs connection status, integration status, tag status, MT1 poses, tag count, and target size for each Limelight via their VisionLogger. MT2 poses are only logged while disabled (they are unreliable when moving). Also updates the Field2d widget with the MT1 pose from each camera.

    • getBackMegaTag1Pose

      public Pose2d getBackMegaTag1Pose()
      Returns the MegaTag1 (MT1) pose from the back Limelight, or Pose2d.kZero if no estimate is available.

    • getLeftMegaTag1Pose

      public Pose2d getLeftMegaTag1Pose()
      Returns the MegaTag1 (MT1) pose from the left Limelight, or Pose2d.kZero if no estimate is available.

    • getRightMegaTag1Pose

      public Pose2d getRightMegaTag1Pose()
      Returns the MegaTag1 (MT1) pose from the right Limelight, or Pose2d.kZero if no estimate is available.

    • getBackMegaTag2Pose

      public Pose2d getBackMegaTag2Pose()
      Returns the MegaTag2 (MT2) pose from the back Limelight, or Pose2d.kZero if no estimate is available.

    • getLeftMegaTag2Pose

      public Pose2d getLeftMegaTag2Pose()
      Returns the MegaTag2 (MT2) pose from the left Limelight, or Pose2d.kZero if no estimate is available.

    • getRightMegaTag2Pose

      public Pose2d getRightMegaTag2Pose()
      Returns the MegaTag2 (MT2) pose from the right Limelight, or Pose2d.kZero if no estimate is available.

    • getBestLimelight

      public Limelight getBestLimelight()
      Selects and returns the Limelight with the highest combined score of visible tag count and target size. Ties fall back to backLL.
      Returns:
      the Limelight currently offering the best view of AprilTags

    • hasAccuratePose

      public boolean hasAccuratePose()
      Returns true if at least one Limelight reports an accurate pose (via Limelight.hasAccuratePose()).

    • tagsInView

      public boolean tagsInView()
      Returns true if any Limelight currently sees an AprilTag belonging to the current alliance's set of scoring targets.

      Uses DriverStation.getAlliance() and falls back to Blue if the alliance is unknown.

    • triggerRewindCaptureForAllCameras

      public void triggerRewindCaptureForAllCameras()
      Triggers a rewind-capture snapshot on all Limelights (captures 165 seconds of history for post-match review).

    • resetPoseToVision

      public void resetPoseToVision()
      Resets the robot pose to the best Limelight's vision pose. Delegates to resetPoseToVision(boolean, Pose3d, Pose2d, double).

    • resetPoseToVision

      public boolean resetPoseToVision(boolean targetInView, Pose3d botpose3D, Pose2d megaPose, double poseTimestamp)
      Resets the robot pose to a specific vision pose if the data passes sanity checks.

      Uses a very tight covariance (0.00001) so the pose estimator snaps immediately to the vision measurement. The MT1 heading is preserved while MT2 provides the translation.

      Rejection criteria:

      • No target in view.
      • Pose outside field boundary.
      • Camera height |z| > 0.25 m (robot floating / bad solve).
      • Roll or pitch > 5° (camera physically disturbed).
      Parameters:
      targetInView - whether the Limelight has a target
      botpose3D - the MT1 Pose3d from the Limelight
      megaPose - the MT2 Pose2d (provides translation for the reset)
      poseTimestamp - the FPGA timestamp of the pose estimate
      Returns:
      true if the pose was accepted and the reset was applied

    • setLimelightPipelines

      public void setLimelightPipelines(int pipeline)
      Sets all Limelights to the given pipeline index.
      Parameters:
      pipeline - the zero-indexed pipeline number to activate

    • blinkLimelights

      public Command blinkLimelights()
      Returns a command that blinks all Limelight LEDs while active and turns them off when the command ends.
      Returns:
      the blink command

    • solidLimelight

      public Command solidLimelight()
      Returns a command that holds all Limelight LEDs solid-on while active and turns them off when the command ends.
      Returns:
      the solid-LED command

    • getBackLL

      public Limelight getBackLL()
      Rear-facing Limelight instance.

    • getLeftLL

      public Limelight getLeftLL()
      Left-facing Limelight instance.

    • getRightLL

      public Limelight getRightLL()
      Right-facing Limelight instance.

    • getTagLayout

      public static AprilTagFieldLayout getTagLayout()
      Field layout loaded once at construction and shared across the robot.