LESSON 23

Vision

Wheel odometry adds up wheel turns, and it drifts. An AprilTag sighting is absolute, occasional, and noisy. This lesson feeds sightings into the pose estimator so the camera pulls odometry back toward the truth.

Branch3-Limelight12 minutes

You’ll need

The swerve project with logging. Branch 3-Limelight is one commit off 2-Logging.
Odometry you trust, from Swerve Calibration. Vision corrects drift, not a wrong wheel radius.
A Limelight bolted to the robot, powered, on the robot network.
An AprilTag. A printed one on a wall works.

Watch out

No camera in simulation

Limelight.java does nothing in the simulator: no camera, so nothing publishes to NetworkTables and the update method returns every loop. Check this page on the real robot.

Camera placement

Every AprilTag carries an ID. The field drawing says where that ID sits, so measuring the tag relative to the camera works backwards to the robot's position.

Mounting decides whether any of this works, and it is the part teams get wrong. Put the camera where it can see the scoring tags while you are scoring. Never mount it level with the tags. You want a tag viewed from an angle: off to one side, and above or below. Dead-on and level gives the worst estimate there is.

Set the camera up

None of the Java below fixes a badly configured camera. Do this on the hardware first, with the robot powered.

Switch the active pipeline to AprilTag. A color-blob pipeline never publishes a botpose.
Drop the exposure as low as it can go while still finding tags. A short shutter cuts motion blur. A blurred tag gives a wrong answer, not no answer.
Enter the camera offsets.Measure where the camera sits relative to the robot's center, and at what angle. Solving gives the camera's pose; the offsets make it the robot's. Get them wrong and every measurement shifts the same way.
Limelight: full 3D tracking
Calibrate the lens with a printed ChArUco board. It corrects lens distortion, worst at the edges of the image. Tags sit there when you are lined up on something.
Limelight: ChArUco calibration
Write down the camera's name. That string is the NetworkTables table it publishes to, and the Java addresses the camera by it. The branch uses the default, limelight.

Hold a tag in front of the camera. The web interface should report its ID.

MegaTag1 and MegaTag2

MegaTag1 solves position and heading from the geometry of the tags in frame. Two or more tags spread across the image constrain that geometry well. One tag does not. A small error in the measured corners swings the solved heading, and the position follows.

MegaTag2 takes your heading as given and solves only for position. One tag is enough. The heading goes in uncorrected, so a gyro ten degrees out returns a position that is wrong and looks fine.

Limelight.java asks for MegaTag1 first. If that estimate is valid but came from one tag, it asks again for MegaTag2.

The validity gate

Copy LimelightHelpers.java from the branch into src/main/java/frc/robot/. Take the branch's copy: it is migrated to the org.wpilib.* packages, and a stock download will not import.

A bad estimate does not fail loudly. It gets folded into odometry and drags the robot's idea of where it is somewhere wrong.

LimelightHelpers.java: the gate

1public static Boolean validPoseEstimate(PoseEstimate pose) {
2  return pose != null && pose.rawFiducials != null && pose.rawFiducials.length != 0;
3}

A fiducial is one detected tag. So: did we get an answer, and did at least one tag go into it? With nothing to read, LimelightHelpers returns a pose at the field origin with an empty fiducial array. A camera that is off or misnamed produces silence, not a robot that thinks it is in a corner.

update() returns early on two conditions and no others: the estimate fails that gate, or avgTagDist is past MAX_TAG_DISTANCE_METERS, set to 4.0 on the branch. Everything else gets through.

Why so few checks

A tag past four meters is a handful of pixels, so that cut is a hard line. Everything nearer is weighted, not rejected: a distant sighting arrives with a large error bar.

The trust weighting

Every sighting goes in with a standard deviation: how far off it might be, in meters and radians. Bigger means trust it less, and the estimator blends the sighting against the wheels in that proportion.

Distance hurts gently and tag count helps hard. The position deviation is XY_STD_DEV_COEFFICIENT * avgTagDist^1.2 / tagCount^2, with the coefficient at 0.333. The heading term scales the same way from ROTATION_STD_DEV_COEFFICIENT, at 1.5. Doubling the distance multiplies the error bar by about 2.3. One tag at two meters gives about 0.77 m. Two tags, same distance, 0.19 m.

The heading MegaTag2 returns

MegaTag2 solved that pose from the heading you gave the camera two lines earlier. Feeding it back as a measurement would be the robot agreeing with itself, growing more confident every loop. So MegaTag2 estimates go in with IGNORE_VISION_HEADING, set to 9_999_999, which the estimator reads as infinity. MegaTag1 heading is a real observation, and gets a real weight.

The measurement goes in with estimate.timestampSeconds, not the current time. The picture was taken, processed, and sent before your code saw it, so the robot has already moved.

All of it lives in src/main/java/frc/robot/subsystems/Limelight.java, about eighty lines. Create that file, then register the camera with one line in Robot's constructor: Limelight.registerAll(drivetrain, "limelight"), plus import frc.robot.subsystems.Limelight;. Not in an OpMode: those bindings are torn down on a mode switch, and vision has to keep correcting in every mode. A second camera is a second string.

Check your work

Deploy, put the robot on the floor with a tag in view, and watch Drivetrain/Pose in NetworkTables.

Park about two meters from a tag and note the pose. Cover the camera and push the robot a meter sideways. The pose follows the wheels.
Uncover the camera. The pose settles toward where the tag says the robot is, over a second rather than in one frame.
Back away past four meters, then close in again. Line up on two tags, then on one.

Check

You should see

The pose walks back to the truth once a tag comes into view, rather than jumping there. Corrections stop past four meters and resume when you close in. With one tag in view, the position moves and the heading does not budge.

Three things go wrong here. A pose that never moves is almost always the name: the string in registerAllmust match the camera's NetworkTables table exactly. A pose that jumps somewhere impossible means the offsets are wrong. If it lands on the far side of the field, a red-origin pose is going into a blue-origin estimator. That is why the class asks for _wpiBlue. Position that corrects on two tags and goes strange on one is the MegaTag2 path, so seed the gyro. Not with seedFieldCentric(), which only changes which way the sticks call forward: Swerve Calibration has the three kinds of zeroing.