Mini-Project: Vision-Aligned Scoring with Limelight

This capstone ties vision to motion: use a Limelight 3 to aim the robot at an AprilTag and to feed field-relative pose into your estimator, then run a command that auto-aligns and scores.

Simple aim: proportional control on tx

Limelight publishes the horizontal angle to the target as tx (degrees). A P-only controller that spins the robot until tx is ~0 is the classic first vision feature. Use the LimelightHelpers wrapper for clean reads:

public Command aimAtTag() {
  final double kP = 0.035;
  return run(() -> {
    double tx = LimelightHelpers.getTX("");
    double omega = -tx * kP;            // degrees -> turn rate
    m_swerve.drive(0, 0, omega);
  }).until(() -> Math.abs(LimelightHelpers.getTX("")) < 1.0);
}

Add a small minimum command so the robot doesn't stall just shy of centered, and a deadband so it doesn't jitter when already aligned.

Robust localization: MegaTag2 fusion

For scoring from a distance you want full field pose, not just an angle. MegaTag2 uses your gyro heading to disambiguate tag pose, dramatically reducing the ambiguity that hurt the original MegaTag. First send the robot's yaw to the Limelight every loop, then pull the estimate and feed it to your SwerveDrivePoseEstimator:

public void updateVision() {
  LimelightHelpers.SetRobotOrientation("",
      m_swerve.getHeading().getDegrees(), 0, 0, 0, 0, 0);

  var mt2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("");
  if (mt2 != null && mt2.tagCount > 0
      && Math.abs(m_gyro.getRate()) < 360) { // ignore while spinning fast (Limelight's documented MegaTag2 rejection threshold)
    m_poseEstimator.setVisionMeasurementStdDevs(
        VecBuilder.fill(0.7, 0.7, 9999999)); // trust x/y, not vision yaw
    m_poseEstimator.addVisionMeasurement(
        mt2.pose, mt2.timestampSeconds);
  }
}

Call updateVision() from the drivetrain's periodic(). Setting the rotation standard deviation huge tells the estimator to ignore vision yaw and keep trusting the gyro -- the standard MegaTag2 pattern, since MegaTag2 relies on your gyro for heading.

The scoring sequence

Now compose: aim, drive to a known scoring pose using your odometry, then trigger the mechanism only when settled:

public Command autoScore(Pose2d scorePose) {
  return aimAtTag()
      .andThen(m_swerve.driveToPose(scorePose))
      .andThen(Commands.waitUntil(m_swerve::atPose))
      .andThen(m_arm.scoreL4());
}

Configure the pipeline correctly

In the Limelight web UI, select AprilTag (Classic 36h11) -- the family FRC has used since 2024 -- and set the tag size to 165.1 mm for current FRC field tags. Limelight pushes targeting data faster than the default NetworkTables rate so you get fresh data each loop. Validate the fused pose in AdvantageScope's Odometry/3D view: the robot's vision pose should snap onto the real field position and stay stable as you drive, with no teleporting when a tag enters or leaves view.