Incredibots Academy

The FTC field is surrounded by AprilTags—QR-code-like squares pasted on the field walls. Because the exact location of every single AprilTag is known, if a camera sees an AprilTag, it can use trigonometry to calculate exactly where the camera is located on the field!

The MegaTag Advantage

Traditionally, if a robot sees one AprilTag, it tries to calculate its position. But if the camera is slightly blurry, or the lighting is weird, the math gets "noisy" and the robot's calculated position might jump around randomly.

The Limelight 3A uses an algorithm called MegaTag. If the Limelight can see multiple AprilTags at the exact same time, it mathematically combines them all into one giant "MegaTag". This drastically reduces noise and gives you an incredibly stable, highly accurate 3D position (X, Y, Z, Pitch, Roll, Yaw) called the BotPose.

Building the Limelight Subsystem

Let's look at how the Unearthed-Alberta repository implements this in their LimelightSubsystem.java.

1. Alliance Specific Pipelines

The team uses the Limelight to look for specific tags depending on their alliance. Red Alliance wants to align with Tag 24, and Blue Alliance wants Tag 20. They configure two different "Pipelines" in the Limelight Web UI (Pipeline 7 for Red, Pipeline 6 for Blue).

public class LimelightSubsystem extends SubsystemBase {
    private final Limelight3A limelight;
    private int targetTagId = -1;
 
    public void setAlliance(boolean isRed) {
        if (isRed) {
            this.targetTagId = 24; // Red Alliance Target
            limelight.pipelineSwitch(7);
        } else {
            this.targetTagId = 20; // Blue Alliance Target
            limelight.pipelineSwitch(6);
        }
        limelight.start();
    }
    // ...

2. Fetching the MegaTag BotPose

Next, the subsystem needs a method to ask the Limelight: "Where am I right now?"

The code fetches the latest result, loops through all the FiducialResults (AprilTags) it saw, and checks if it saw the targetTagId. If it did, it grabs the MegaTag BotPose!

    /**
     * Returns the robot's pose on the field based on AprilTags.
     * Returns null if no tags are in view.
     */
    public Pose getLatestFieldPose() {
        // Grab the latest data packet from the Limelight
        LLResult result = limelight.getLatestResult();
 
        if (result != null && result.isValid()) {
            // Get a list of every AprilTag the camera is currently looking at
            List<LLResultTypes.FiducialResult> fiducials = result.getFiducialResults();
 
            boolean targetSeen = false;
            
            // Loop through them to see if our Alliance's tag is in the frame
            for (LLResultTypes.FiducialResult f : fiducials) {
                if (f.getFiducialId() == targetTagId) {
                    targetSeen = true;
                    break;
                }
            }
 
            // If we see our target tag, extract the MegaTag BotPose!
            if (targetSeen) {
                Pose3D botpose = result.getBotpose();
                
                if (botpose != null) {
                    // ... Conversion logic goes here ...
                }
            }
        }
        return null;
    }

3. Converting to PedroPathing

The Limelight returns a Pose3D object (X, Y, Z coordinates). However, our driving system (PedroPathing) uses its own custom 2D Pose object (X, Y, Heading).

Worse, the standard FTC Coordinate system is rotated differently than the PedroPathing Coordinate system! We have to translate it using a helper provided by PedroPathing:

                if (botpose != null) {
                    // 1. Extract X, Y, and Yaw (Heading) from the 3D Pose
                    // 2. Tell PedroPathing this is an FTCCoordinate
                    // 3. Ask it to convert it to a PedroCoordinate!
                    return new Pose(
                            botpose.getPosition().toUnit(DistanceUnit.INCH).x,
                            botpose.getPosition().toUnit(DistanceUnit.INCH).y,
                            botpose.getOrientation().getYaw(AngleUnit.RADIANS),
                            FTCCoordinates.INSTANCE).getAsCoordinateSystem(PedroCoordinates.INSTANCE);
                }

Once you have this translated Pose, you can inject it directly into your Odometry system using follower.setStartingPose(), instantly correcting any drift that happened while the robot was driving!

Check Your Understanding

Check yourself

Localization with MegaTag