redScoring redAudience blueAudience blueScoring found redCenterDistance r4Seen r5Seen b4Seen b5Seen R1Distance R2Distance R3Distance R4Distance R5Distance R6Distance R7Distance R8Distance B1Distance B2Distance B3Distance B4Distance B5Distance B6Distance B7Distance B8Distance result selecting list selectedId selectedRange tag redX pose redY blueX redZ blueY forward x r4X r5X redRange blueZ strafe y r4Y r5Y b4X b5X blueRange currentRange turn z r4Z r5Z redTx b4Y b5Y blueTx currentTx speed b4Z b5Z yaw denominator pitch range id tx group offset clusterOffset clusterX clusterY clusterZ clusterRange r4Tx r5Tx b4Tx b5Tx scanTags SCAN APRILTAGS This helper procedure asks the Limelight what it can see. It converts tag poses into distances, uses official tag spacing to estimate hidden R4/R5 or B4/B5 positions, and calculates a center point. WHY RESET EVERY SCAN? 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. redScoring not visible redAudience not visible blueAudience not visible blueScoring not visible found FALSE redCenterDistance not available r4Seen FALSE r5Seen FALSE b4Seen FALSE b5Seen FALSE R1Distance not visible R2Distance not visible R3Distance not visible R4Distance not visible R5Distance not visible R6Distance not visible R7Distance not visible R8Distance not visible B1Distance not visible B2Distance not visible B3Distance not visible B4Distance not visible B5Distance not visible B6Distance not visible B7Distance not visible B8Distance not visible CAMERA RESULT AND LIST 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. result limelightAsLimelight3A {"IDENTIFIER":"limelight"} result list FiducialResults result THE CAMERA COORDINATE SYSTEM 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. tag list pose TargetPoseCameraSpace tag POSITION NUMBERS 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. x position.x pose y position.y pose z position.z pose ANGLE NUMBERS 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. yaw orientation.yaw pose pitch orientation.pitch pose PYTHAGOREAN DISTANCE IN 3D 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 MULTIPLY ROOT ADD ADD MULTIPLY x x MULTIPLY y y MULTIPLY z z 39.3701 id FiducialId tag tx TargetXDegrees tag FOUR HALF-GROUPS 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. group -1 offset 0 TAG NUMBERS AND KNOWN OFFSETS 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. EQ id 30 R1Distance range group 0 offset 13 clusterOffset 6.5 EQ id 31 R2Distance range group 0 offset 9.25 clusterOffset 2.75 EQ id 32 R3Distance range group 0 offset 3.75 clusterOffset -2.75 EQ id 33 R4Distance range group 0 offset 0 clusterOffset -6.5 EQ id 34 R5Distance range group 1 offset 0 clusterOffset 6.5 EQ id 35 R6Distance range group 1 offset -3.75 clusterOffset 2.75 EQ id 36 R7Distance range group 1 offset -9.25 clusterOffset -2.75 EQ id 37 R8Distance range group 1 offset -13 clusterOffset -6.5 EQ id 38 B1Distance range group 2 offset 13 clusterOffset 6.5 EQ id 39 B2Distance range group 2 offset 9.25 clusterOffset 2.75 EQ id 40 B3Distance range group 2 offset 3.75 clusterOffset -2.75 EQ id 41 B4Distance range group 2 offset 0 clusterOffset -6.5 EQ id 42 B5Distance range group 3 offset 0 clusterOffset 6.5 EQ id 43 B6Distance range group 3 offset -3.75 clusterOffset 2.75 EQ id 44 B7Distance range group 3 offset -9.25 clusterOffset -2.75 EQ id 45 B8Distance range group 3 offset -13 clusterOffset -6.5 WHY SINE AND COSINE? 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. CLUSTER CENTER DISTANCE 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. clusterX ADD x MULTIPLY DIVIDE clusterOffset 39.3701 MULTIPLY COS yaw COS pitch clusterY ADD y MULTIPLY DIVIDE clusterOffset 39.3701 MULTIPLY SIN yaw COS pitch clusterZ MINUS z MULTIPLY DIVIDE clusterOffset 39.3701 SIN pitch clusterRange MULTIPLY ROOT ADD ADD MULTIPLY clusterX clusterX MULTIPLY clusterY clusterY MULTIPLY clusterZ clusterZ 39.3701 EQ group 0 redScoring clusterRange EQ group 1 redAudience clusterRange EQ group 2 blueAudience clusterRange EQ group 3 blueScoring clusterRange EQ group 0 r4X ADD x MULTIPLY DIVIDE offset 39.3701 MULTIPLY COS yaw COS pitch r4Y ADD y MULTIPLY DIVIDE offset 39.3701 MULTIPLY SIN yaw COS pitch r4Z MINUS z MULTIPLY DIVIDE offset 39.3701 SIN pitch r4Tx tx r4Seen TRUE EQ group 1 r5X ADD x MULTIPLY DIVIDE offset 39.3701 MULTIPLY COS yaw COS pitch r5Y ADD y MULTIPLY DIVIDE offset 39.3701 MULTIPLY SIN yaw COS pitch r5Z MINUS z MULTIPLY DIVIDE offset 39.3701 SIN pitch r5Tx tx r5Seen TRUE EQ group 2 b4X ADD x MULTIPLY DIVIDE offset 39.3701 MULTIPLY COS yaw COS pitch b4Y ADD y MULTIPLY DIVIDE offset 39.3701 MULTIPLY SIN yaw COS pitch b4Z MINUS z MULTIPLY DIVIDE offset 39.3701 SIN pitch b4Tx tx b4Seen TRUE EQ group 3 b5X ADD x MULTIPLY DIVIDE offset 39.3701 MULTIPLY COS yaw COS pitch b5Y ADD y MULTIPLY DIVIDE offset 39.3701 MULTIPLY SIN yaw COS pitch b5Z MINUS z MULTIPLY DIVIDE offset 39.3701 SIN pitch b5Tx tx b5Seen TRUE HOW CENTER DISTANCE AND ANGLE WORK 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. AND r4Seen r5Seen redX DIVIDE ADD r4X r5X 2 redY DIVIDE ADD r4Y r5Y 2 redZ DIVIDE ADD r4Z r5Z 2 redRange MULTIPLY ROOT ADD ADD MULTIPLY redX redX MULTIPLY redY redY MULTIPLY redZ redZ 39.3701 redCenterDistance redRange redTx redZ redX AND b4Seen b5Seen blueX DIVIDE ADD b4X b5X 2 blueY DIVIDE ADD b4Y b5Y 2 blueZ DIVIDE ADD b4Z b5Z 2 blueRange MULTIPLY ROOT ADD ADD MULTIPLY blueX blueX MULTIPLY blueY blueY MULTIPLY blueZ blueZ 39.3701 blueTx blueZ blueX FOUND, SELECTED ID, AND CURRENT VALUES 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. selecting AND AND r4Seen r5Seen LT redRange selectedRange selectedRange redRange selectedId 0 currentRange redRange currentTx redTx found TRUE AND AND b4Seen b5Seen LT blueRange selectedRange selectedRange blueRange selectedId 1 currentRange blueRange currentTx blueTx found TRUE AND EQ selectedId 0 AND r4Seen r5Seen currentRange redRange currentTx redTx found TRUE AND EQ selectedId 1 AND b4Seen b5Seen currentRange blueRange currentTx blueTx found TRUE runOpMode APRILTAG TELEOP This program combines normal mecanum driving with Limelight vision. During INIT it shows red tag distances. While driving, hold Square to make the robot face a calculated cluster center. Release Square to return to joystick control. {"commentPositionLeft":-115,"commentPositionTop":-125} ROBOT SETUP: Configure motor directions, braking, and encoder mode. Reversing the left motors makes positive power mean forward on both sides. front_left_driveAsDcMotor Direction {"IDENTIFIER":"front_left_drive"} REVERSE back_left_driveAsDcMotor Direction {"IDENTIFIER":"back_left_drive"} REVERSE front_right_driveAsDcMotor Direction {"IDENTIFIER":"front_right_drive"} FORWARD back_right_driveAsDcMotor Direction {"IDENTIFIER":"back_right_drive"} REVERSE front_left_driveAsDcMotor ZeroPowerBehavior {"IDENTIFIER":"front_left_drive"} BRAKE front_right_driveAsDcMotor ZeroPowerBehavior {"IDENTIFIER":"front_right_drive"} BRAKE back_left_driveAsDcMotor ZeroPowerBehavior {"IDENTIFIER":"back_left_drive"} BRAKE back_right_driveAsDcMotor ZeroPowerBehavior {"IDENTIFIER":"back_right_drive"} BRAKE front_left_driveAsDcMotor Mode {"IDENTIFIER":"front_left_drive"} RUN_WITHOUT_ENCODER front_right_driveAsDcMotor Mode {"IDENTIFIER":"front_right_drive"} RUN_WITHOUT_ENCODER back_left_driveAsDcMotor Mode {"IDENTIFIER":"back_left_drive"} RUN_WITHOUT_ENCODER back_right_driveAsDcMotor Mode {"IDENTIFIER":"back_right_drive"} RUN_WITHOUT_ENCODER front_left_driveAsDcMotor Power {"IDENTIFIER":"front_left_drive"} 0 front_right_driveAsDcMotor Power {"IDENTIFIER":"front_right_drive"} 0 back_left_driveAsDcMotor Power {"IDENTIFIER":"back_left_drive"} 0 back_right_driveAsDcMotor Power {"IDENTIFIER":"back_right_drive"} 0 INIT: red tag and triangulated center distances limelightAsLimelight3A {"IDENTIFIER":"limelight"} 100 limelightAsLimelight3A {"IDENTIFIER":"limelight"} 0 START THE CAMERA: Pipeline 0 looks for AprilTags. Starting the Limelight here lets INIT and Square alignment use fresh vision results. limelightAsLimelight3A {"IDENTIFIER":"limelight"} Hold Square: align to triangulated cluster center INIT VISION: Before START, repeatedly scan tags and show R1-R8 plus the calculated red-center distance. Motor powers stay at zero during INIT. WHILE R1 R1Distance R2 R2Distance R3 R3Distance R4 R4Distance R5 R5Distance R6 R6Distance R7 R7Distance R8 R8Distance Red center distance (in) redCenterDistance WAIT FOR START: The robot is ready, but this block prevents driving until the Driver Station START button is pressed. WHILE selecting TRUE selectedId -1 selectedRange 9999 MANUAL DRIVE: Read the joysticks every loop. Left stick drives and strafes; right stick X rotates; left bumper chooses slow mode. forward NEG gamepad1 LeftStickY {"IDENTIFIER":"gamepad1"} strafe gamepad1 LeftStickX {"IDENTIFIER":"gamepad1"} turn gamepad1 RightStickX {"IDENTIFIER":"gamepad1"} speed 1 gamepad1 LeftBumper {"IDENTIFIER":"gamepad1"} speed 0.35 denominator ADD ADD ABS forward ABS strafe ABS turn LT denominator 1 denominator 1 WHEEL POWERS: Mecanum drive mixes forward, strafe, and turn differently for each wheel. Dividing by the denominator keeps every command in the safe -1 to +1 range. front_left_driveAsDcMotor Power {"IDENTIFIER":"front_left_drive"} MULTIPLY speed DIVIDE ADD ADD forward strafe turn denominator front_right_driveAsDcMotor Power {"IDENTIFIER":"front_right_drive"} MULTIPLY speed DIVIDE MINUS MINUS forward strafe turn denominator back_left_driveAsDcMotor Power {"IDENTIFIER":"back_left_drive"} MULTIPLY speed DIVIDE ADD MINUS forward strafe turn denominator back_right_driveAsDcMotor Power {"IDENTIFIER":"back_right_drive"} MULTIPLY speed DIVIDE MINUS ADD forward strafe turn denominator WHAT SQUARE DOES 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. gamepad1 Square {"IDENTIFIER":"gamepad1"} found LTE ABS currentTx 2 front_left_driveAsDcMotor Power {"IDENTIFIER":"front_left_drive"} 0 back_left_driveAsDcMotor Power {"IDENTIFIER":"back_left_drive"} 0 front_right_driveAsDcMotor Power {"IDENTIFIER":"front_right_drive"} 0 back_right_driveAsDcMotor Power {"IDENTIFIER":"back_right_drive"} 0 FOUR CLUSTER DISTANCES 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. Red scoring cluster (in) redScoring Red audience cluster (in) redAudience Blue audience cluster (in) blueAudience Blue scoring cluster (in) blueScoring DRIVER STATION DISPLAY: Send commands, motor powers, buttons, and Square-alignment information to the Driver Station. Square align CENTERED GT currentTx 0 front_left_driveAsDcMotor Power {"IDENTIFIER":"front_left_drive"} 0.18 back_left_driveAsDcMotor Power {"IDENTIFIER":"back_left_drive"} 0.18 front_right_driveAsDcMotor Power {"IDENTIFIER":"front_right_drive"} -0.24 back_right_driveAsDcMotor Power {"IDENTIFIER":"back_right_drive"} -0.18 Square align turning to center front_left_driveAsDcMotor Power {"IDENTIFIER":"front_left_drive"} -0.18 back_left_driveAsDcMotor Power {"IDENTIFIER":"back_left_drive"} -0.18 front_right_driveAsDcMotor Power {"IDENTIFIER":"front_right_drive"} 0.24 back_right_driveAsDcMotor Power {"IDENTIFIER":"back_right_drive"} 0.18 Square align turning to center Center error (deg) currentTx Center distance (in) currentRange front_left_driveAsDcMotor Power {"IDENTIFIER":"front_left_drive"} 0 back_left_driveAsDcMotor Power {"IDENTIFIER":"back_left_drive"} 0 front_right_driveAsDcMotor Power {"IDENTIFIER":"front_right_drive"} 0 back_right_driveAsDcMotor Power {"IDENTIFIER":"back_right_drive"} 0 Square align need one visible tag from each cluster half Forward command forward Strafe command strafe Rotate command turn Speed scale speed FL motor power front_left_driveAsDcMotor Power {"IDENTIFIER":"front_left_drive"} FR motor power front_right_driveAsDcMotor Power {"IDENTIFIER":"front_right_drive"} BL motor power back_left_driveAsDcMotor Power {"IDENTIFIER":"back_left_drive"} BR motor power back_right_driveAsDcMotor Power {"IDENTIFIER":"back_right_drive"} Face buttons A: gamepad1 A {"IDENTIFIER":"gamepad1"} B: gamepad1 B {"IDENTIFIER":"gamepad1"} X: gamepad1 X {"IDENTIFIER":"gamepad1"} Y: gamepad1 Y {"IDENTIFIER":"gamepad1"} D-pad U: gamepad1 DpadUp {"IDENTIFIER":"gamepad1"} D: gamepad1 DpadDown {"IDENTIFIER":"gamepad1"} L: gamepad1 DpadLeft {"IDENTIFIER":"gamepad1"} R: gamepad1 DpadRight {"IDENTIFIER":"gamepad1"} Other buttons LB: gamepad1 LeftBumper {"IDENTIFIER":"gamepad1"} RB: gamepad1 RightBumper {"IDENTIFIER":"gamepad1"} Start: gamepad1 Start {"IDENTIFIER":"gamepad1"} Back: gamepad1 Back {"IDENTIFIER":"gamepad1"} SAFETY STOP: After STOP is pressed, set all motors to zero and stop the Limelight. front_left_driveAsDcMotor Power {"IDENTIFIER":"front_left_drive"} 0 front_right_driveAsDcMotor Power {"IDENTIFIER":"front_right_drive"} 0 back_left_driveAsDcMotor Power {"IDENTIFIER":"back_left_drive"} 0 back_right_driveAsDcMotor Power {"IDENTIFIER":"back_right_drive"} 0 limelightAsLimelight3A {"IDENTIFIER":"limelight"}