Skip to main content
VisionSubsystem fuses pose measurements from three Limelights — limelight-a, limelight-b, and limelight-c — into the drivebase pose estimator via drivetrain.addVisionMeasurement(). Both MegaTag1 (MT1) and MegaTag2 (MT2) solve modes are processed each cycle. Each camera maintains its own last-accepted pose independently for the per-solve-mode velocity plausibility gate.

​
Camera positions (comp bot)

All transforms are robot-origin → camera-lens, using WPILib axes (X forward, Y left, Z up):

​
MT1 vs MT2

MT2 provides more accurate XY at the cost of heading independence. MT1 provides heading corrections when multiple tags are visible.

​
Measurement pipeline (processLimelight)

Each measurement goes through a sequential rejection pipeline. The first failed check publishes a _rejectReason string to SmartDashboard and discards the measurement.
1

Stale-timestamp check

If estimate.timestampSeconds == camera.getLastTimestampSeconds() the Limelight has not returned a new frame. Reason: stale-timestamp.
2

Alpha bot velocity gate

On the alpha bot only: if |vx|, |vy| > 2.0 m/s or |ω| > 0.2 rad/s, the measurement is dropped. Reason: alpha-max-speed.
3

Impossible rotation / height check

If the pose 3D roll or pitch exceeds 12°, or the 2D pose matches the last accepted pose exactly, the measurement is discarded. Reason: impossible-rotation or height.
4

Field boundary check

Pose must be within the field boundary plus a 0.5 m margin on all sides. Reason: off-field.
5

Velocity plausibility check

Vision-implied speed (distance between consecutive accepted poses / dt) must be below 7.0 m/s normally, or 12.0 m/s under defense. Reason: velocity-implausible.
6

Inter-tag spread check

RMS error between per-tag independent distance estimates and the reported pose must be below 0.50 m. If any tag ID is unknown, the entire observation is hard-rejected. Reason: inter-tag-inconsistent.
7

Ambiguity / range gate

Std dev computation returns MAX_VALUE (⟹ measurement weight ≈ 0) if ambiguity ≥ 0.4, or if a single-tag observation exceeds the range gate (MT1: 2.0 m, MT2: 5.0 m). Reason: ambiguity-or-range.
8

Accepted — addVisionMeasurement

drivetrain.addVisionMeasurement(visionPose2d, fpgaTimestamp, stdDevs) is called. The per-camera last-accepted pose and timestamp are updated.

​
Standard deviation formula

Where:
  • A_XY_MT2 = 0.07 (default, tunable via /vision/A_XY_MT2)
  • A_XY_MT1 = 0.09 (default, tunable via /vision/A_XY_MT1)
  • P_XY = 1.4 (distance exponent, tunable via /vision/P_XY)
  • harmonicSum = Σ 1/dist² over all visible tags
  • ambiguityInflation = 1 / (1 - maxAmbiguity)²
If spread > SPREAD_INFLATE_START (0.10 m), std devs are further multiplied by (spread / 0.10)².

​
Defense detection

isUnderDefense() fires when either of two conditions is true: When under defense, std devs are multiplied by DEFENSE_STD_DEV_SCALE = 0.5, halving std devs and quadrupling the filter weight — making the pose estimator trust vision more aggressively during contact.

​
Auto odometry reset

If the drivebase pose walks off the field boundary AND a camera simultaneously sees ≥ 2 tags with ambiguity < 0.15, drivetrain.resetTranslation(visionPose.getTranslation()) is called to snap the odometry back onto the field. Heading is preserved to avoid fighting the gyro.

​
Limelight online check

isLimeLightOnline(String name) reads the hb (heartbeat) NetworkTables entry from the Limelight’s table. The camera is considered online only if the entry’s last-change timestamp is within 1.0 second of the current FPGA time.

​
Public API

​
Disabling vision fusion

Set the NetworkTables topic /vision/disablevision to true to stop all addVisionMeasurement calls. All three cameras will report vision-disabled as their rejection reason. This is useful when vision data is actively degrading the pose estimate (e.g., near a reflective surface).
The alpha bot uses a stricter velocity gate: MAX_XY_VELO_ALPHA = 2.0 m/s and MAX_TURN_VELO_ALPHA = 0.2 rad/s. Vision measurements are rejected at much lower speeds on the alpha bot than on the competition bot (where no hard per-axis velocity limit is enforced — only the vision-to-vision implied-speed gate applies).
VisionSubsystem publishes per-camera SmartDashboard entries for the last timestamp, number of targets, time since last reading, rejection reason, vision pose error (compared to the pose history at the same timestamp), and per-camera MT1/MT2 std dev values. These are useful during field calibration to verify all three cameras are contributing valid measurements.