found redCenterDistance r4Seen r5Seen b4Seen b5Seen R1Distance R2Distance R3Distance R4Distance R5Distance R6Distance R7Distance R8Distance B1Distance B2Distance B3Distance B4Distance B5Distance B6Distance B7Distance B8Distance result list selecting tag redX selectedId pose redY blueX selectedRange redZ blueY x r4X r5X redRange blueZ y r4Y r5Y b4X b5X blueRange currentRange z r4Z r5Z redTx b4Y b5Y blueTx currentTx b4Z b5Z yaw centered pitch range id tx group offset startRange targetRange done r4Tx r5Tx b4Tx b5Tx runOpMode APRILTAG AUTONOMOUS 1. During INIT, show red tag and center distances. 2. After START, find the nearer red or blue cluster center. 3. Rotate until centered. 4. Drive toward the center and stop 24 inches away. 5. Stop safely if vision is lost or a timeout occurs. {"commentPositionLeft":161,"commentPositionTop":-13} ROBOT SETUP: Configure directions, BRAKE behavior, encoder mode, and zero motor power before anything can move. 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"} FORWARD 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_USING_ENCODER front_right_driveAsDcMotor Mode {"IDENTIFIER":"front_right_drive"} RUN_USING_ENCODER back_left_driveAsDcMotor Mode {"IDENTIFIER":"back_left_drive"} RUN_USING_ENCODER back_right_driveAsDcMotor Mode {"IDENTIFIER":"back_right_drive"} RUN_USING_ENCODER 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 MsTransmissionInterval 50 limelightAsLimelight3A {"IDENTIFIER":"limelight"} 100 limelightAsLimelight3A {"IDENTIFIER":"limelight"} 0 START THE LIMELIGHT: Pipeline 0 detects AprilTags. A fast poll rate gives the control loops frequent steering updates. limelightAsLimelight3A {"IDENTIFIER":"limelight"} Status READY: triangulate center and stop 24 inches away INIT DISPLAY: Scan continuously before START. Show R1-R8 and the triangulated red-center distance while all motor powers remain zero. WHILE INIT vision Showing red tags; triangulating red and blue centers WAIT FOR START: Autonomous movement is blocked until the Driver Station START button is pressed. selecting TRUE selectedId -1 selectedRange 9999 ACQUIRE MEANS FIND AND CHOOSE For up to 3 seconds, scan for enough information to build a center: at least one visible tag from each half of an alliance. The program may see many tags, but it chooses the nearer complete red or blue center. The timeout prevents an endless loop when no usable target exists. WHILE AND AND LT selectedId 0 LT 3 Phase Acquiring nearest AprilTag Selected tag selectedId GTE selectedId 0 selecting FALSE centered 0 CENTER USING A HORIZONTAL ANGLE currentTx means the target’s horizontal X angle in degrees; Tx here does not mean transmit. Positive and negative tell which way to turn. Within 2 degrees counts as centered. Requiring 5 centered camera frames filters flicker: one lucky noisy reading cannot end the turn early. WHILE AND AND LT centered 5 LT 6 found LTE ABS currentTx 2 centered ADD centered 1 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 GT currentTx 0 centered 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 centered 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 Phase Centering on selected AprilTag Horizontal error (deg) currentTx Centered frames centered 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 Status Selected tag not visible; motors stopped 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 GTE centered 5 startRange currentRange targetRange 24 GTE targetRange 10 done FALSE DRIVE TO A 24-INCH RANGE Range means straight-line distance from camera to calculated center. The robot drives while currentRange is greater than 24 inches and stops at 24 or less. Small unequal left/right powers correct the horizontal angle while moving. The 8-second timeout and found flag stop the robot if vision fails. WHILE AND AND done LT 8 found LTE currentRange targetRange 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 done TRUE GT currentTx 2 front_left_driveAsDcMotor Power {"IDENTIFIER":"front_left_drive"} 0.3 back_left_driveAsDcMotor Power {"IDENTIFIER":"back_left_drive"} 0.3 front_right_driveAsDcMotor Power {"IDENTIFIER":"front_right_drive"} 0.26 back_right_driveAsDcMotor Power {"IDENTIFIER":"back_right_drive"} 0.2 LT currentTx -2 front_left_driveAsDcMotor Power {"IDENTIFIER":"front_left_drive"} 0.2 back_left_driveAsDcMotor Power {"IDENTIFIER":"back_left_drive"} 0.2 front_right_driveAsDcMotor Power {"IDENTIFIER":"front_right_drive"} 0.36 back_right_driveAsDcMotor Power {"IDENTIFIER":"back_right_drive"} 0.3 front_left_driveAsDcMotor Power {"IDENTIFIER":"front_left_drive"} 0.25 back_left_driveAsDcMotor Power {"IDENTIFIER":"back_left_drive"} 0.25 front_right_driveAsDcMotor Power {"IDENTIFIER":"front_right_drive"} 0.31 back_right_driveAsDcMotor Power {"IDENTIFIER":"back_right_drive"} 0.25 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 Status Selected tag not visible; motors stopped Phase Driving toward center; stopping at 24 inches Start range (in) startRange Current range (in) currentRange Distance traveled (in) MINUS startRange 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 done Result COMPLETE: centered and reached 24-inch range Result STOPPED: approach timeout or tag unavailable 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 Result STOPPED: already within 24-inch center range 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 Result STOPPED: unable to center before timeout 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 Result STOPPED: no AprilTag acquired 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 FINAL SAFETY STOP: Set every motor to zero and stop the Limelight when autonomous finishes. limelightAsLimelight3A {"IDENTIFIER":"limelight"} scanTags SCAN APRILTAGS This helper reads the Limelight, labels BIOBUZZ tags, estimates hidden R4/R5 or B4/B5 endpoints with trigonometry, and calculates each cluster center. FRESH FRAME, FRESH FLAGS Every camera frame is a new measurement. Reset found and visible flags before reading it. Otherwise a tag that disappeared could leave an old true value behind, making the robot steer using stale information. 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 WHAT THE LIMELIGHT RETURNS getLatestResult retrieves the newest processed image result. Fiducial means a known visual marker. FiducialResults is a list of all recognized AprilTags, and the for-each loop performs the same geometry on each list item. result limelightAsLimelight3A {"IDENTIFIER":"limelight"} result list FiducialResults result CAMERA COORDINATES AND POSE Pose means position plus rotation. X is left/right, Y is up/down, and Z is forward from the camera. Yaw is left/right rotation; pitch is up/down tilt. All are needed because the tag strip may be viewed from an angle. tag list pose TargetPoseCameraSpace tag SIGNED X, Y, Z VALUES A signed number contains direction and size. For example, negative X may mean left and positive X right. Squaring removes the signs for distance, but keeping signs before that is essential for steering and projecting hidden points. x position.x pose y position.y pose z position.z pose DEGREES AND ROTATION Yaw is like turning your head left/right; pitch is like nodding up/down. The Limelight gives degrees. The Blocks sine and cosine operations convert these degree angles internally, so students do not need to calculate radians. yaw orientation.yaw pose pitch orientation.pitch pose WHY SQUARE, ADD, THEN SQUARE ROOT? Each axis is one side of a 3D right-angle path. Squaring makes every contribution positive. Adding X²+Y²+Z² combines all three dimensions. The square root returns the direct camera-to-tag distance. Multiply meters by 39.3701 for inches. range MULTIPLY ROOT ADD ADD MULTIPLY x x MULTIPLY y y MULTIPLY z z 39.3701 id FiducialId tag tx TargetXDegrees tag WHY FOUR GROUP NUMBERS? 0 means red first half R1–R4; 1 means red second half R5–R8; 2 and 3 are the matching blue halves. One visible tag in group 0 plus one in group 1 can build the red center. Groups 2 and 3 build blue. group -1 offset 0 BIOBUZZ TAG MAP AND OFFSETS IDs 30–37 are red R1–R8; IDs 38–45 are blue B1–B8. Tag centers within each four-tag strip sit at -6.5, -2.75, +2.75, and +6.5 inches. Those known measurements let one visible tag estimate a hidden endpoint. EQ id 30 R1Distance range group 0 offset 13 EQ id 31 R2Distance range group 0 offset 9.25 EQ id 32 R3Distance range group 0 offset 3.75 EQ id 33 R4Distance range group 0 offset 0 EQ id 34 R5Distance range group 1 offset 0 EQ id 35 R6Distance range group 1 offset -3.75 EQ id 36 R7Distance range group 1 offset -9.25 EQ id 37 R8Distance range group 1 offset -13 EQ id 38 B1Distance range group 2 offset 13 EQ id 39 B2Distance range group 2 offset 9.25 EQ id 40 B3Distance range group 2 offset 3.75 EQ id 41 B4Distance range group 2 offset 0 EQ id 42 B5Distance range group 3 offset 0 EQ id 43 B6Distance range group 3 offset -3.75 EQ id 44 B7Distance range group 3 offset -9.25 EQ id 45 B8Distance range group 3 offset -13 ROTATE A KNOWN OFFSET WITH TRIGONOMETRY An offset measured along the tag strip is not automatically camera-left/right when the strip is angled. Cosine gives the component along an axis; sine gives the perpendicular component. Using yaw and pitch rotates the offset into camera X, Y, and Z. Divide inches by 39.3701 first because pose coordinates are meters. 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 AprilTags none detected MIDPOINT, DISTANCE, AND DIRECTION Average endpoint coordinates to find the center: (A+B)/2 on each axis. Then distance=square root of X²+Y²+Z², the 3D Pythagorean theorem. atan2(X,Z) gives the signed left/right angle even when X or Z is zero; ordinary tangent division could fail or lose the correct quadrant. 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 TARGET STATE VARIABLES selectedId uses 0 for red center and 1 for blue center; it is not a physical tag ID. currentRange is center distance in inches. currentTx is center horizontal angle in degrees. found must be true before any vision-guided motor command is allowed. 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 R1 R1Distance R2 R2Distance R3 R3Distance R4 R4Distance R5 R5Distance R6 R6Distance R7 R7Distance R8 R8Distance Red center distance (in) requires both tags redCenterDistance