FRC Multi-Limelight Vision Template

简体中文

A reusable WPILib Java vision subsystem for one or more Limelight cameras. It was extracted from an FRC 2026 competition robot and reorganized so the vision logic is independent of the drivetrain vendor and season mechanisms.

This is more than a botpose reader. It includes:

• Quality-tiered MegaTag1 primary localization with MegaTag2 fallback
• Disabled-only MegaTag1 field-pose bootstrap
• Multi-camera heading recovery after a gyro reset or heading fault
• Multi-camera translation recovery after large odometry drift
• Camera quality scoring, hysteresis, and stale-frame rejection
• A primary-camera aiming mode with fresh-frame and alliance-tag interlocks
• Hailo-8, Hailo-8L, Coral, and CPU neural-detector result access
• NetworkTables telemetry for rejection reasons and recovery state
• Unit tests for the recovery and safety state machines

Warning

This template can intentionally reset field heading or translation after strict multi-camera checks. Keep additional cameras disabled until every camera is calibrated and its robot-space pose is verified. Test on blocks and at low speed before enabling automatic recovery on a full robot.

Architecture

flowchart LR
    Gyro["Gyro health and yaw rate"] --> Orientation["Send robot orientation"]
    Orientation --> Cameras["Limelight cameras"]
    Cameras --> MT1["MegaTag1 observations"]
    Cameras --> MT2["MegaTag2 observations"]
    MT1 --> Bootstrap["Disabled pose bootstrap"]
    MT1 --> Fast["Explicit fast relocalization"]
    MT1 --> Heading["Absolute heading and hard recovery"]
    MT2 --> Translation["Translation recovery consensus"]
    MT1 --> Selection["Per-camera source selection"]
    MT2 --> Selection
    Selection --> Fusion["Timestamped pose fusion"]
    Fusion --> Aim["Camera selection and aiming pose"]
    Bootstrap --> Drive["VisionDrivetrain"]
    Fast --> Drive
    Heading --> Drive
    Translation --> Drive
    Fusion --> Drive

Loading

High-quality multi-tag MegaTag1 is the normal absolute XY and field-heading source. Medium-quality multi-tag MegaTag1 still supplies XY, but heading needs agreement from a second camera. Single-tag or poor-geometry frames fall back to gyro-seeded MegaTag2. Each camera contributes at most one of MT1 or MT2 to the estimator per poll. Primary aiming consumes vision poses but does not implicitly reset odometry.

IRIS V2 behavior changes

Area	Previous V2	MT1-primary V2
Normal localization	Every accepted camera fused MT2 XY.	High/medium multi-tag MT1 supplies XY; high-quality or corroborated medium MT1 also supplies low-gain field heading.
MT2 use	Normal source whenever the Gyro was healthy.	Same-camera fallback for single-tag, missing, or poor-geometry MT1 only.
Heading correction	Separate low-speed, 10 Hz trim; autonomous was telemetry-only.	Part of the same capture-timestamp MT1 measurement in Disabled, Teleop, and Auto; translation speed increases covariance instead of rejecting the frame.
Unhealthy Gyro	All normal vision localization stopped.	MT2 remains forbidden; reliable multi-tag MT1 may still be fused, while localization remains marked unreliable.
Duplicate solver measurements	MT1 trim and MT2 XY could both represent one camera poll.	Exactly one MT1 or MT2 measurement per camera poll reaches the estimator.
Bootstrap, aim, Fast Relocalization, hard recovery	Present.	Preserved with the same explicit reset and reset-epoch rules.
One-camera startup	Supported.	High-quality MT1 can fuse heading; medium-quality MT1 remains XY-only without corroboration.

Requirements

• WPILib 2026 Java
• Limelight OS 2026.0 or newer
• LimelightHelpers v1.14, included in this repository
• A drivetrain pose estimator with timestamped vision measurement support
• Gyro yaw-rate and health information for MegaTag2 and reliability gating

The core package has no CTRE, REV, PathPlanner, or Choreo dependency.

Repository layout

src/main/java/frc/robot/
├── LimelightHelpers.java
├── Main.java
├── Robot.java
└── vision/
    ├── Vision.java
    ├── VisionConstants.java
    ├── VisionDrivetrain.java
    ├── Limelight.java
    ├── NeuralDetector.java
    ├── AimCameraSelector.java
    ├── VisionQuality.java
    ├── VisionAimManager.java
    ├── FastRelocalizationManager.java
    ├── VisionPoseBootstrapManager.java
    ├── HeadingRecoveryManager.java
    ├── TranslationRecoveryManager.java
    └── VisionRecoveryMath.java

Robot.java is intentionally minimal. Normally, copy frc.robot.vision and LimelightHelpers.java into an existing robot project.

Quick start

1. Configure the season

Edit VisionConstants.java:

1. Update the WPILib year, GradleRIO version, and vendordep for the new season.
2. Set every Limelight hostname in kCameras.
3. Leave a camera disabled until its calibration and mounting pose are verified.
4. Select kPrimaryAimCameraName.
5. Replace the red and blue aim-target AprilTag IDs.
6. Replace the red and blue aim-target field positions.
7. Confirm that AprilTagFields.kDefaultField is the correct field layout.

The included aim-target values are the original 2026 example, not universal defaults.

2. Configure each Limelight

In the Limelight web interface:

1. Set the FRC team number and a unique hostname.
2. Calibrate the camera.
3. Enter the camera pose in robot space.
4. Select the correct official field map.
5. Use an AprilTag pipeline for localization.
6. Confirm that the robot-space axes and camera orientation are correct.

Use a wired network connection and give each camera clean, protected power.

For a competition robot, configure and enable all three real camera hostnames. Verify each camera's calibration, robot-space pose, field map, heartbeat, power, and network path independently before enabling it. The public template may leave cameras two and three disabled so a one-camera checkout still starts. Medium-quality heading and hard recovery naturally wait for enough agreeing cameras; high-quality normal MT1 and explicitly requested fast relocalization can still use a qualified frame from the primary camera alone.

3. Implement the drivetrain boundary

Make the drivetrain implement VisionDrivetrain. The important mappings are:

public final class Swerve extends YourDrivetrainBase
    implements Subsystem, VisionDrivetrain {

    @Override
    public Pose2d getPose() {
        return getState().Pose;
    }

    @Override
    public ChassisSpeeds getRobotRelativeSpeeds() {
        return getState().Speeds == null
            ? new ChassisSpeeds()
            : getState().Speeds;
    }

    @Override
    public Optional<Pose2d> samplePoseAtFpgaTime(double timestampSeconds) {
        // Phoenix 6 example; other estimators may already use FPGA time.
        return super.samplePoseAt(Utils.fpgaToCurrentTime(timestampSeconds));
    }

    @Override
    public void addVisionMeasurement(
        Pose2d pose,
        double timestampSeconds,
        Matrix<N3, N1> standardDeviations
    ) {
        poseEstimator.addVisionMeasurement(
            pose,
            timestampSeconds,
            standardDeviations
        );
    }
}

Implement resetPoseFromVision(Pose2d) for complete vision resets. Keep the translation-only and heading-only reset methods separate. Track a reset sequence, timestamp, and source whenever odometry is reset so vision evidence captured before a reset cannot be reused afterward.

getGyroHealth() should return cached or non-blocking gyro signals. Do not perform slow CAN reads in the vision loop. Aggregate vendor-specific fatal fault bits into the record's hardFault field.

4. Construct the subsystem

private final Swerve swerve = new Swerve();
private final Vision vision = new Vision(swerve);

Because Vision extends SubsystemBase, the command scheduler calls its periodic() method automatically.

5. Use the aiming safety API

Start and end a primary-camera aiming session with the command lifecycle:

@Override
public void initialize() {
    vision.beginPrimaryAim();
}

@Override
public void execute() {
    Optional<Pose2d> preparationPose = vision.getAimPreparationPose();
    Optional<Pose2d> freshShotPose = vision.getFreshAimPose();

    // preparationPose may be used for steering and mechanism warmup.
    // Only freshShotPose should authorize the final feed/fire action.
}

@Override
public void end(boolean interrupted) {
    vision.setAimSuccessful(false);
    vision.endPrimaryAim();
}

For the final shot interlock, combine getFreshAimPose() with hasRecentAllianceAimTargetTag() and your own heading/mechanism tolerances.

6. Request fast relocalization explicitly

Request a full-pose correction only when the command or autonomous routine has evidence that odometry may be wrong:

vision.requestFastRelocalization();

The request expires after 0.20 s and can reset at most once. It does not wait for low speed, block aiming, or stop shooter and hood warmup. Keep beginPrimaryAim() and mechanism preparation on their normal command lifecycle; starting an aim session alone never requests relocalization.

Neural object detection

NeuralDetector reads the standard Limelight rawdetections output. The robot code is identical whether inference is accelerated by Hailo-8, Hailo-8L, Google Coral, or the Limelight CPU.

private final NeuralDetector detector = new NeuralDetector("limelight");

var target = detector.getLargestDetection(0);
target.ifPresent(detection -> {
    double horizontalErrorDegrees = detection.txnc;
    double verticalErrorDegrees = detection.tync;
    double imageAreaPercent = detection.ta;
});

Class IDs follow the labels file uploaded with the model. Selecting the largest detection is a simple nearest-object heuristic; use getClosestToCrosshair() when target continuity matters more.

This repository deliberately stops at perception. Put drivetrain actuation in a separate command so losing a target can immediately brake or return control to the driver.

Safety behavior

The template maintains these invariants:

• No MegaTag2 use while gyro orientation is unhealthy.
• High-quality multi-tag MegaTag1 may correct XY and the estimator field heading; medium-quality heading requires a second agreeing camera.
• Heading errors of 10° or more are withheld from normal heading fusion and routed to hard recovery.
• Translation speed scales heading uncertainty instead of blocking MT1; heading is suppressed above 120°/s.
• Disabled pose bootstrap requires stable multi-frame MegaTag1 evidence.
• Automatic hard recovery is blocked during autonomous and while moving quickly; an explicit fast-relocalization request is the autonomous recovery path.
• Heading and translation recovery require multiple agreeing cameras over time.
• Evidence is cleared after every pose-reset epoch.
• A fresh frame is required for the final aiming/fire interlock.
• Additional cameras start disabled in the public template.

Do not weaken several recovery thresholds at once. Log the rejection reason, change one limit, and repeat the same test.

V2 parameter changes and log limits

Parameter	V1	V2	Evidence
Normal MegaTag2 single-tag maximum ambiguity	0.42	0.45	Offline prac6 replay adds 10 auto frames and 119 enabled-match frames across three cameras.
Recovery maximum frame age	0.15 s	0.20 s	The observed maximum was about 0.13 s; the extra margin covers scheduler jitter.
High-quality MT1 geometry	N/A	>=2 tags, >=1.0 m span, <=0.15 ambiguity, <=4.5 m, <=0.20 s	Conservative initial continuous-fusion gate; validate with current robot logs.
Medium-quality MT1 geometry	N/A	>=2 tags, >=0.50 m span, <=0.24 ambiguity, <=5.4 m, <=0.30 s	XY remains useful; heading needs two-camera agreement.
Normal MT1 heading yaw rate	N/A	<=120°/s	Faster rotation keeps qualified XY but ignores heading.

The 2.4 m normal innovation gate, distance and area gates, normal 0.30 s frame age, 0.05 s future tolerance, heartbeat timeout, and 720°/s angular velocity limit remain unchanged. The replay showed negligible benefit from loosening them, and older logs include a dangerous roughly 8.5 m (0,0) pose error.

prac6 predates the current Primary MegaTag1 raw-pose telemetry. It supports the threshold and timing-budget decisions above, but cannot validate fast relocalization, single-tag PnP rejection, or reset-before/reset-after behavior. Those paths still require a new robot log containing Primary MT1 pose, tags, ambiguity, distance, request state, correction result, and both reset poses. No real-robot V2 acceptance is claimed by this repository change.

NetworkTables telemetry

Summary data is published under Vision/Summary, including:

• selected aim camera, quality, and pose
• field translation/heading initialization state
• localization reliability
• bootstrap state, reason, evidence count, and selected pose
• heading and translation recovery state and correction
• fast-relocalization state, rejection reason, evidence, correction, and timeout
• the selected MT1/MT2 path
• last pose-reset source and timestamp
• gyro health and field-heading offset

Per-camera data is published under Vision/<camera-name>, including the final localization source and reason, MT1 tag span/ambiguity/distance, time-aligned MT1/MT2 XY difference, the actual selected XY/heading standard deviations, Gyro health, Auto/Teleop application flags, and source acceptance/rejection counts.

These keys are intended for AdvantageScope or Shuffleboard debugging.

Testing

Run:

env JAVA_HOME=/Users/yumi/wpilib/2026/jdk \
PATH=/Users/yumi/wpilib/2026/jdk/bin:/usr/bin:/bin:/usr/sbin:/sbin \
./gradlew build --rerun-tasks --console=plain

Use the WPILib-provided JDK if the system Java version is newer than the GradleRIO-supported version. The tests cover camera selection, statistical quality, MT1/MT2 localization policy, bootstrap stability, heading and translation recovery, fast relocalization, aiming freshness, alliance-tag filtering, and neural target selection.

Before enabling recovery on a robot, also test:

1. One camera disconnected.
2. One camera mounted with a deliberately wrong pose.
3. Gyro reset while disabled.
4. Gyro reset while enabled and stationary.
5. Large odometry translation error.
6. Fast rotation and fast translation.
7. Autonomous trajectory execution.
8. Tag loss immediately before a shot.

Updating LimelightHelpers

LimelightHelpers.java comes from the official LimelightVision/limelightlib-wpijava repository. Replace it with the newest official release when updating Limelight OS or WPILib, then run the complete test suite.

License and attribution

The reusable template code is released under the BSD 3-Clause License in LICENSE. WPILib-generated files retain the WPILib license in WPILib-License.md. LimelightHelpers.java is distributed under Limelight Vision's MIT license; see THIRD_PARTY_NOTICES.md.