Manual driving, live cluster distances, and Square-button alignment using AprilTag geometry.
Before running: Ask your mentor first. Keep Driver Station STOP ready.
Read these steps before exploring the full program. The numbered blue comments in the diagram link back to the matching step.
A camera produces many separate pictures called frames. A tag visible in the previous frame might disappear in the next one. We begin with found=false and not visible values so yesterday’s—or even 1/100-second-old—data cannot steer the robot by mistake.
getLatestResult asks for the newest processed camera frame. The result can contain zero, one, or many tags. Fiducial is the camera word for a known visual marker such as an AprilTag. The list lets us repeat the same math for every marker found.
For each tag, X is left/right, Y is up/down, and Z is forward/back from the camera. The pose also has yaw and pitch angles. Yaw means turning left/right; pitch means tilting up/down. These are geometry terms—not wireless transmit or receive abbreviations.
X, Y, and Z are distances measured from the camera to the tag center. They are signed numbers: the sign tells which direction, while the absolute size tells how far along that axis. Keeping all three axes lets the program measure a diagonal line through 3D space.
Yaw rotates around the up/down axis, like shaking your head “no.” Pitch rotates up/down, like nodding “yes.” The Limelight reports degrees. Sine and cosine blocks expect degrees here, so no student-written radians conversion is needed.
In 2D, a right triangle has hypotenuse square root of X²+Z². The camera also has height Y, so we add Y²: square root of X²+Y²+Z². Multiply meters by 39.3701 to display inches.
range = √(X² + Y² + Z²)Three perpendicular camera measurements combine into one straight-line range.
Group 0 estimates R4 from R1–R4. Group 1 estimates R5 from R5–R8. Group 2 estimates B4, and group 3 estimates B5. One visible tag from each matching pair of groups is enough to calculate that alliance center.
Red tags are IDs 30–37; blue tags are IDs 38–45. Each four-tag strip has centers at -6.5, -2.75, +2.75, and +6.5 inches from its middle. Example: R1 is 13 inches left of R4, so seeing R1 lets us estimate where hidden R4 should be.
The known tag offset lies along the tag strip, but the camera may view that strip at an angle. Cosine tells how much of the offset points along one camera axis; sine tells the perpendicular part. We use yaw and pitch to rotate the offset into camera X, Y, and Z. Inches are divided by 39.3701 because the camera pose uses meters.
ΔX = d·cos(yaw)·cos(pitch) ΔY = d·sin(yaw)·cos(pitch)Sine and cosine split one known strip distance into camera-axis pieces.
A cluster is one sticker containing four AprilTags. clusterOffset moves from whichever tag is visible to that sticker’s middle. Sine and cosine rotate the offset into camera coordinates. Then square root of X²+Y²+Z² gives distance to the cluster center, not distance to one tag.
ΔX = d·cos(yaw)·cos(pitch) ΔY = d·sin(yaw)·cos(pitch)Sine and cosine split one known strip distance into camera-axis pieces.
First average the estimated endpoints: center=(end1+end2)/2 for X, Y, and Z. Then use the 3D Pythagorean theorem: distance=square root of X²+Y²+Z². atan2(X,Z) answers “what left/right angle points from the camera to that center?” This angle replaces the raw tag horizontal angle.
center = (A + B) ÷ 2 angle = atan2(X, Z)Average two reconstructed endpoints, then calculate the signed turn toward their center.
found is a true/false safety flag. selectedId is 0 for the red center or 1 for the blue center; it is not an AprilTag number. currentRange stores inches to the chosen center. currentTx stores its horizontal angle in degrees; here Tx means target X angle, not transmit.
Square temporarily overrides the joystick motor powers. The program scans the tags, calculates a center, and turns until the center is within 2 degrees of straight ahead. A degree is 1/360 of a full circle. Releasing Square immediately gives control back to the driver. If the center cannot be calculated, all motor powers become zero.
measure → turn → measure again → stop inside ±2°The robot repeatedly corrects fresh angle error instead of guessing one perfect turn.
These rows are live after START. Red Scoring means IDs 30–33, Red Audience 34–37, Blue Audience 38–41, and Blue Scoring 42–45. “not visible” means no tag from that sticker was found in the newest camera frame.
Zoom in to follow the connections. Click a blue comment or focus its label and press Enter to revisit that lesson step.
Ask your mentor before importing or running these Blocks. A download does not set up or start the robot.
Safety: Read and predict before running. Verify the required hardware and configuration. Raise the wheels for the first motor test, keep one person ready to press STOP, and never rely on camera code as the only safety system.