AprilTag Pose + Aim Assist

Disclaimer: WIP and untested on 2026 hardware; tune gains and stale timeout on your robot. Custom code built on WPILib APIs (not official WPILib sample).

Purpose: Use AprilTags to align to hub/tower; fall back to odometry if vision is stale.

Code (Java/WPILib + PhotonVision style)

public class VisionAim {
  private final PhotonCamera cam = new PhotonCamera("photonvision");
  private final PIDController turnPid = new PIDController(0.02, 0.0, 0.001);
  private final double maxTurn = 0.5; // clamp
  private double lastYaw = 0.0;
  private double lastTime = 0.0;

  public OptionalDouble getYawToTarget() {
    var res = cam.getLatestResult();
    if (!res.hasTargets()) return OptionalDouble.empty();
    PhotonTrackedTarget target = res.getBestTarget();
    lastYaw = target.getYaw();
    lastTime = Timer.getFPGATimestamp();
    return OptionalDouble.of(lastYaw);
  }

  public double turnCommand() {
    double yaw = getYawToTarget().orElse(lastYaw);
    double out = turnPid.calculate(yaw, 0.0);
    return MathUtil.clamp(out, -maxTurn, maxTurn);
  }

  public boolean stale(double maxAgeSec) {
    return (Timer.getFPGATimestamp() - lastTime) > maxAgeSec;
  }
}

Usage

VisionAim vision = new VisionAim();

// In teleop, button to aim:
double turn = vision.turnCommand();
if (vision.stale(0.5)) {
  // fallback: use gyro/odometry instead of vision
  turn = 0.0;
}
drive.arcadeDrive(fwd, turn);

Parameters to tune

• PID gains and maxTurn.
• Max age before considering vision stale.
• Camera name and pipeline settings for AprilTags.

How to test

• Bench: point robot at a tag; verify yaw reading and turn output sign.
• Field: rotate 360°, ensure stale detection kicks in when tag lost.
• With drivetrain on blocks, command turn and observe smoothness/clamp.

Pitfalls

• Ensure camera is configured for AprilTags with correct layout.
• Clamp turn to avoid oscillation; add slew if needed.