Autonomous // Advanced
AprilTagAuto
Triangulates a cluster center, turns toward it, approaches, and stops 24 inches away.
Visual source + comment guide
Explore the Blocks
The diagram stays close to FTC Blocks while the guide wraps every full student comment for easy reading.
Safety: Read and predict before running. 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.
Show generated JavaScript
This is what FTC Blocks generates for the Robot Controller. The visual Blocks above are the primary student source.
// IDENTIFIERS_USED=back_left_driveAsDcMotor,back_right_driveAsDcMotor,front_left_driveAsDcMotor,front_right_driveAsDcMotor,limelightAsLimelight3A
var 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;
/**
* 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.
*/
function runOpMode() {
front_left_driveAsDcMotor.setDirection("REVERSE");
back_left_driveAsDcMotor.setDirection("REVERSE");
front_right_driveAsDcMotor.setDirection("FORWARD");
back_right_driveAsDcMotor.setDirection("FORWARD");
front_left_driveAsDcMotor.setZeroPowerBehavior("BRAKE");
front_right_driveAsDcMotor.setZeroPowerBehavior("BRAKE");
back_left_driveAsDcMotor.setZeroPowerBehavior("BRAKE");
back_right_driveAsDcMotor.setZeroPowerBehavior("BRAKE");
front_left_driveAsDcMotor.setMode("RUN_USING_ENCODER");
front_right_driveAsDcMotor.setMode("RUN_USING_ENCODER");
back_left_driveAsDcMotor.setMode("RUN_USING_ENCODER");
back_right_driveAsDcMotor.setMode("RUN_USING_ENCODER");
front_left_driveAsDcMotor.setPower(0);
back_left_driveAsDcMotor.setPower(0);
front_right_driveAsDcMotor.setPower(0);
back_right_driveAsDcMotor.setPower(0);
telemetry.setMsTransmissionInterval(50);
limelightAsLimelight3A.setPollRateHz(100);
limelightAsLimelight3A.pipelineSwitch(0);
limelightAsLimelight3A.start();
telemetryAddTextData('Status', 'READY: triangulate center and stop 24 inches away');
telemetry.update();
while (linearOpMode.opModeInInit()) {
scanTags();
telemetryAddTextData('INIT vision', 'Showing red tags; triangulating red and blue centers');
telemetry.update();
linearOpMode.idle();
}
linearOpMode.waitForStart();
selecting = true;
selectedId = -1;
selectedRange = 9999;
linearOpMode.resetRuntime();
while (linearOpMode.opModeIsActive() && selectedId < 0 && linearOpMode.getRuntime() < 3) {
scanTags();
telemetryAddTextData('Phase', 'Acquiring nearest AprilTag');
telemetry.addNumericData('Selected tag', selectedId);
telemetry.update();
linearOpMode.idle();
}
if (selectedId >= 0) {
selecting = false;
centered = 0;
linearOpMode.resetRuntime();
while (linearOpMode.opModeIsActive() && centered < 5 && linearOpMode.getRuntime() < 6) {
scanTags();
if (found) {
if (Math.abs(currentTx) <= 2) {
centered = centered + 1;
front_left_driveAsDcMotor.setPower(0);
back_left_driveAsDcMotor.setPower(0);
front_right_driveAsDcMotor.setPower(0);
back_right_driveAsDcMotor.setPower(0);
} else {
if (currentTx > 0) {
centered = 0;
front_left_driveAsDcMotor.setPower(0.18);
back_left_driveAsDcMotor.setPower(0.18);
front_right_driveAsDcMotor.setPower(-0.24);
back_right_driveAsDcMotor.setPower(-0.18);
} else {
centered = 0;
front_left_driveAsDcMotor.setPower(-0.18);
back_left_driveAsDcMotor.setPower(-0.18);
front_right_driveAsDcMotor.setPower(0.24);
back_right_driveAsDcMotor.setPower(0.18);
}
}
telemetryAddTextData('Phase', 'Centering on selected AprilTag');
telemetry.addNumericData('Horizontal error (deg)', currentTx);
telemetry.addNumericData('Centered frames', centered);
} else {
front_left_driveAsDcMotor.setPower(0);
back_left_driveAsDcMotor.setPower(0);
front_right_driveAsDcMotor.setPower(0);
back_right_driveAsDcMotor.setPower(0);
telemetryAddTextData('Status', 'Selected tag not visible; motors stopped');
}
telemetry.update();
linearOpMode.idle();
}
front_left_driveAsDcMotor.setPower(0);
back_left_driveAsDcMotor.setPower(0);
front_right_driveAsDcMotor.setPower(0);
back_right_driveAsDcMotor.setPower(0);
if (centered >= 5) {
startRange = currentRange;
targetRange = 24;
if (targetRange >= 10) {
done = false;
linearOpMode.resetRuntime();
while (linearOpMode.opModeIsActive() && !done && linearOpMode.getRuntime() < 8) {
scanTags();
if (found) {
if (currentRange <= targetRange) {
front_left_driveAsDcMotor.setPower(0);
back_left_driveAsDcMotor.setPower(0);
front_right_driveAsDcMotor.setPower(0);
back_right_driveAsDcMotor.setPower(0);
done = true;
} else {
if (currentTx > 2) {
front_left_driveAsDcMotor.setPower(0.3);
back_left_driveAsDcMotor.setPower(0.3);
front_right_driveAsDcMotor.setPower(0.26);
back_right_driveAsDcMotor.setPower(0.2);
} else {
if (currentTx < -2) {
front_left_driveAsDcMotor.setPower(0.2);
back_left_driveAsDcMotor.setPower(0.2);
front_right_driveAsDcMotor.setPower(0.36);
back_right_driveAsDcMotor.setPower(0.3);
} else {
front_left_driveAsDcMotor.setPower(0.25);
back_left_driveAsDcMotor.setPower(0.25);
front_right_driveAsDcMotor.setPower(0.31);
back_right_driveAsDcMotor.setPower(0.25);
}
}
}
} else {
front_left_driveAsDcMotor.setPower(0);
back_left_driveAsDcMotor.setPower(0);
front_right_driveAsDcMotor.setPower(0);
back_right_driveAsDcMotor.setPower(0);
telemetryAddTextData('Status', 'Selected tag not visible; motors stopped');
}
telemetryAddTextData('Phase', 'Driving toward center; stopping at 24 inches');
telemetry.addNumericData('Start range (in)', startRange);
telemetry.addNumericData('Current range (in)', currentRange);
telemetry.addNumericData('Distance traveled (in)', startRange - currentRange);
telemetry.update();
linearOpMode.idle();
}
front_left_driveAsDcMotor.setPower(0);
back_left_driveAsDcMotor.setPower(0);
front_right_driveAsDcMotor.setPower(0);
back_right_driveAsDcMotor.setPower(0);
if (done) {
telemetryAddTextData('Result', 'COMPLETE: centered and reached 24-inch range');
} else {
telemetryAddTextData('Result', 'STOPPED: approach timeout or tag unavailable');
}
telemetry.update();
} else {
front_left_driveAsDcMotor.setPower(0);
back_left_driveAsDcMotor.setPower(0);
front_right_driveAsDcMotor.setPower(0);
back_right_driveAsDcMotor.setPower(0);
telemetryAddTextData('Result', 'STOPPED: already within 24-inch center range');
telemetry.update();
}
} else {
front_left_driveAsDcMotor.setPower(0);
back_left_driveAsDcMotor.setPower(0);
front_right_driveAsDcMotor.setPower(0);
back_right_driveAsDcMotor.setPower(0);
telemetryAddTextData('Result', 'STOPPED: unable to center before timeout');
telemetry.update();
}
} else {
front_left_driveAsDcMotor.setPower(0);
back_left_driveAsDcMotor.setPower(0);
front_right_driveAsDcMotor.setPower(0);
back_right_driveAsDcMotor.setPower(0);
telemetryAddTextData('Result', 'STOPPED: no AprilTag acquired');
telemetry.update();
}
front_left_driveAsDcMotor.setPower(0);
back_left_driveAsDcMotor.setPower(0);
front_right_driveAsDcMotor.setPower(0);
back_right_driveAsDcMotor.setPower(0);
limelightAsLimelight3A.stop();
}
/**
* 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.
*/
function scanTags() {
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';
result = limelightAsLimelight3A.getLatestResult();
if (result != null) {
list = startBlockExecution("LLResult.FiducialResults") ? endBlockExecution(getObjectViaJson(miscAccess,result).FiducialResults) : 0;
for (var tag_index in list) {
tag = list[tag_index];
pose = startBlockExecution("FiducialResult.TargetPoseCameraSpace") ? endBlockExecution(tag.targetPoseCameraSpace) : 0;
x = startBlockExecution("Pose3D.position.x") ? endBlockExecution(pose.position.x) : 0;
y = startBlockExecution("Pose3D.position.y") ? endBlockExecution(pose.position.y) : 0;
z = startBlockExecution("Pose3D.position.z") ? endBlockExecution(pose.position.z) : 0;
yaw = startBlockExecution("Pose3D.orientation.yaw") ? endBlockExecution(pose.orientation.yaw) : 0;
pitch = startBlockExecution("Pose3D.orientation.pitch") ? endBlockExecution(pose.orientation.pitch) : 0;
range = Math.sqrt(x * x + y * y + z * z) * 39.3701;
id = startBlockExecution("FiducialResult.FiducialId") ? endBlockExecution(tag.fiducialId) : 0;
tx = startBlockExecution("FiducialResult.TargetXDegrees") ? endBlockExecution(tag.targetXDegrees) : 0;
group = -1;
offset = 0;
if (id == 30) {
R1Distance = range;
group = 0;
offset = 13;
}
if (id == 31) {
R2Distance = range;
group = 0;
offset = 9.25;
}
if (id == 32) {
R3Distance = range;
group = 0;
offset = 3.75;
}
if (id == 33) {
R4Distance = range;
group = 0;
offset = 0;
}
if (id == 34) {
R5Distance = range;
group = 1;
offset = 0;
}
if (id == 35) {
R6Distance = range;
group = 1;
offset = -3.75;
}
if (id == 36) {
R7Distance = range;
group = 1;
offset = -9.25;
}
if (id == 37) {
R8Distance = range;
group = 1;
offset = -13;
}
if (id == 38) {
B1Distance = range;
group = 2;
offset = 13;
}
if (id == 39) {
B2Distance = range;
group = 2;
offset = 9.25;
}
if (id == 40) {
B3Distance = range;
group = 2;
offset = 3.75;
}
if (id == 41) {
B4Distance = range;
group = 2;
offset = 0;
}
if (id == 42) {
B5Distance = range;
group = 3;
offset = 0;
}
if (id == 43) {
B6Distance = range;
group = 3;
offset = -3.75;
}
if (id == 44) {
B7Distance = range;
group = 3;
offset = -9.25;
}
if (id == 45) {
B8Distance = range;
group = 3;
offset = -13;
}
if (group == 0) {
r4X = x + (offset / 39.3701) * Math.cos(yaw / 180 * Math.PI) * Math.cos(pitch / 180 * Math.PI);
r4Y = y + (offset / 39.3701) * Math.sin(yaw / 180 * Math.PI) * Math.cos(pitch / 180 * Math.PI);
r4Z = z - (offset / 39.3701) * Math.sin(pitch / 180 * Math.PI);
r4Tx = tx;
r4Seen = true;
}
if (group == 1) {
r5X = x + (offset / 39.3701) * Math.cos(yaw / 180 * Math.PI) * Math.cos(pitch / 180 * Math.PI);
r5Y = y + (offset / 39.3701) * Math.sin(yaw / 180 * Math.PI) * Math.cos(pitch / 180 * Math.PI);
r5Z = z - (offset / 39.3701) * Math.sin(pitch / 180 * Math.PI);
r5Tx = tx;
r5Seen = true;
}
if (group == 2) {
b4X = x + (offset / 39.3701) * Math.cos(yaw / 180 * Math.PI) * Math.cos(pitch / 180 * Math.PI);
b4Y = y + (offset / 39.3701) * Math.sin(yaw / 180 * Math.PI) * Math.cos(pitch / 180 * Math.PI);
b4Z = z - (offset / 39.3701) * Math.sin(pitch / 180 * Math.PI);
b4Tx = tx;
b4Seen = true;
}
if (group == 3) {
b5X = x + (offset / 39.3701) * Math.cos(yaw / 180 * Math.PI) * Math.cos(pitch / 180 * Math.PI);
b5Y = y + (offset / 39.3701) * Math.sin(yaw / 180 * Math.PI) * Math.cos(pitch / 180 * Math.PI);
b5Z = z - (offset / 39.3701) * Math.sin(pitch / 180 * Math.PI);
b5Tx = tx;
b5Seen = true;
}
}
} else {
telemetryAddTextData('AprilTags', 'none detected');
}
if (r4Seen && r5Seen) {
redX = (r4X + r5X) / 2;
redY = (r4Y + r5Y) / 2;
redZ = (r4Z + r5Z) / 2;
redRange = Math.sqrt(redX * redX + redY * redY + redZ * redZ) * 39.3701;
redCenterDistance = redRange;
redTx = Math.atan2(redX, redZ) / Math.PI * 180;
}
if (b4Seen && b5Seen) {
blueX = (b4X + b5X) / 2;
blueY = (b4Y + b5Y) / 2;
blueZ = (b4Z + b5Z) / 2;
blueRange = Math.sqrt(blueX * blueX + blueY * blueY + blueZ * blueZ) * 39.3701;
blueTx = Math.atan2(blueX, blueZ) / Math.PI * 180;
}
if (selecting) {
if (r4Seen && r5Seen && redRange < selectedRange) {
selectedRange = redRange;
selectedId = 0;
currentRange = redRange;
currentTx = redTx;
found = true;
}
if (b4Seen && b5Seen && blueRange < selectedRange) {
selectedRange = blueRange;
selectedId = 1;
currentRange = blueRange;
currentTx = blueTx;
found = true;
}
} else {
if (selectedId == 0 && r4Seen && r5Seen) {
currentRange = redRange;
currentTx = redTx;
found = true;
}
if (selectedId == 1 && b4Seen && b5Seen) {
currentRange = blueRange;
currentTx = blueTx;
found = true;
}
}
telemetryAddTextData('R1', R1Distance);
telemetryAddTextData('R2', R2Distance);
telemetryAddTextData('R3', R3Distance);
telemetryAddTextData('R4', R4Distance);
telemetryAddTextData('R5', R5Distance);
telemetryAddTextData('R6', R6Distance);
telemetryAddTextData('R7', R7Distance);
telemetryAddTextData('R8', R8Distance);
telemetryAddTextData('Red center distance (in)', redCenterDistance);
}