TeleOp + Vision // Next challenge
AprilTagTeleOp
Manual driving, live cluster distances, and Square-button alignment using AprilTag geometry.
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,gamepad1,limelightAsLimelight3A
var 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, forward2, 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;
/**
* 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.
*/
function scanTags() {
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';
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;
clusterOffset = 6.5;
}
if (id == 31) {
R2Distance = range;
group = 0;
offset = 9.25;
clusterOffset = 2.75;
}
if (id == 32) {
R3Distance = range;
group = 0;
offset = 3.75;
clusterOffset = -2.75;
}
if (id == 33) {
R4Distance = range;
group = 0;
offset = 0;
clusterOffset = -6.5;
}
if (id == 34) {
R5Distance = range;
group = 1;
offset = 0;
clusterOffset = 6.5;
}
if (id == 35) {
R6Distance = range;
group = 1;
offset = -3.75;
clusterOffset = 2.75;
}
if (id == 36) {
R7Distance = range;
group = 1;
offset = -9.25;
clusterOffset = -2.75;
}
if (id == 37) {
R8Distance = range;
group = 1;
offset = -13;
clusterOffset = -6.5;
}
if (id == 38) {
B1Distance = range;
group = 2;
offset = 13;
clusterOffset = 6.5;
}
if (id == 39) {
B2Distance = range;
group = 2;
offset = 9.25;
clusterOffset = 2.75;
}
if (id == 40) {
B3Distance = range;
group = 2;
offset = 3.75;
clusterOffset = -2.75;
}
if (id == 41) {
B4Distance = range;
group = 2;
offset = 0;
clusterOffset = -6.5;
}
if (id == 42) {
B5Distance = range;
group = 3;
offset = 0;
clusterOffset = 6.5;
}
if (id == 43) {
B6Distance = range;
group = 3;
offset = -3.75;
clusterOffset = 2.75;
}
if (id == 44) {
B7Distance = range;
group = 3;
offset = -9.25;
clusterOffset = -2.75;
}
if (id == 45) {
B8Distance = range;
group = 3;
offset = -13;
clusterOffset = -6.5;
}
clusterX = x + (clusterOffset / 39.3701) * Math.cos(yaw / 180 * Math.PI) * Math.cos(pitch / 180 * Math.PI);
clusterY = y + (clusterOffset / 39.3701) * Math.sin(yaw / 180 * Math.PI) * Math.cos(pitch / 180 * Math.PI);
clusterZ = z - (clusterOffset / 39.3701) * Math.sin(pitch / 180 * Math.PI);
clusterRange = Math.sqrt(clusterX * clusterX + clusterY * clusterY + clusterZ * clusterZ) * 39.3701;
if (group == 0) {
redScoring = clusterRange;
}
if (group == 1) {
redAudience = clusterRange;
}
if (group == 2) {
blueAudience = clusterRange;
}
if (group == 3) {
blueScoring = clusterRange;
}
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 {
}
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;
}
}
}
/**
* 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.
*/
function runOpMode() {
front_left_driveAsDcMotor.setDirection("REVERSE");
back_left_driveAsDcMotor.setDirection("REVERSE");
front_right_driveAsDcMotor.setDirection("FORWARD");
back_right_driveAsDcMotor.setDirection("REVERSE");
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_WITHOUT_ENCODER");
front_right_driveAsDcMotor.setMode("RUN_WITHOUT_ENCODER");
back_left_driveAsDcMotor.setMode("RUN_WITHOUT_ENCODER");
back_right_driveAsDcMotor.setMode("RUN_WITHOUT_ENCODER");
front_left_driveAsDcMotor.setPower(0);
front_right_driveAsDcMotor.setPower(0);
back_left_driveAsDcMotor.setPower(0);
back_right_driveAsDcMotor.setPower(0);
telemetry.addLine('INIT: red tag and triangulated center distances');
telemetry.update();
limelightAsLimelight3A.setPollRateHz(100);
limelightAsLimelight3A.pipelineSwitch(0);
limelightAsLimelight3A.start();
telemetry.addLine('Hold Square: align to triangulated cluster center');
while (linearOpMode.opModeInInit()) {
scanTags();
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);
telemetry.update();
linearOpMode.idle();
}
linearOpMode.waitForStart();
while (linearOpMode.opModeIsActive()) {
selecting = true;
selectedId = -1;
selectedRange = 9999;
scanTags();
forward2 = -gamepad1.getLeftStickY();
strafe = gamepad1.getLeftStickX();
turn = gamepad1.getRightStickX();
speed = 1;
if (gamepad1.getLeftBumper()) {
speed = 0.35;
}
denominator = Math.abs(forward2) + Math.abs(strafe) + Math.abs(turn);
if (denominator < 1) {
denominator = 1;
}
front_left_driveAsDcMotor.setPower(speed * ((forward2 + strafe + turn) / denominator));
front_right_driveAsDcMotor.setPower(speed * (((forward2 - strafe) - turn) / denominator));
back_left_driveAsDcMotor.setPower(speed * (((forward2 - strafe) + turn) / denominator));
back_right_driveAsDcMotor.setPower(speed * (((forward2 + strafe) - turn) / denominator));
if (gamepad1.getSquare()) {
if (found) {
if (Math.abs(currentTx) <= 2) {
front_left_driveAsDcMotor.setPower(0);
back_left_driveAsDcMotor.setPower(0);
front_right_driveAsDcMotor.setPower(0);
back_right_driveAsDcMotor.setPower(0);
telemetryAddTextData('Red scoring cluster (in)', redScoring);
telemetryAddTextData('Red audience cluster (in)', redAudience);
telemetryAddTextData('Blue audience cluster (in)', blueAudience);
telemetryAddTextData('Blue scoring cluster (in)', blueScoring);
telemetryAddTextData('Square align', 'CENTERED');
} else {
if (currentTx > 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('Square align', 'turning to center');
} else {
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('Square align', 'turning to center');
}
}
telemetryAddTextData('Center error (deg)', currentTx);
telemetryAddTextData('Center distance (in)', currentRange);
} else {
front_left_driveAsDcMotor.setPower(0);
back_left_driveAsDcMotor.setPower(0);
front_right_driveAsDcMotor.setPower(0);
back_right_driveAsDcMotor.setPower(0);
telemetryAddTextData('Square align', 'need one visible tag from each cluster half');
}
}
telemetry.addNumericData('Forward command', forward2);
telemetry.addNumericData('Strafe command', strafe);
telemetry.addNumericData('Rotate command', turn);
telemetry.addNumericData('Speed scale', speed);
telemetry.addNumericData('FL motor power', front_left_driveAsDcMotor.getPower());
telemetry.addNumericData('FR motor power', front_right_driveAsDcMotor.getPower());
telemetry.addNumericData('BL motor power', back_left_driveAsDcMotor.getPower());
telemetry.addNumericData('BR motor power', back_right_driveAsDcMotor.getPower());
telemetryAddTextData('Face buttons', ['A:',gamepad1.getA(),' B:',gamepad1.getB(),' X:',gamepad1.getX(),' Y:',gamepad1.getY()].join(''));
telemetryAddTextData('D-pad', ['U:',gamepad1.getDpadUp(),' D:',gamepad1.getDpadDown(),' L:',gamepad1.getDpadLeft(),' R:',gamepad1.getDpadRight()].join(''));
telemetryAddTextData('Other buttons', ['LB:',gamepad1.getLeftBumper(),' RB:',gamepad1.getRightBumper(),' Start:',gamepad1.getStart(),' Back:',gamepad1.getBack()].join(''));
telemetry.update();
}
front_left_driveAsDcMotor.setPower(0);
front_right_driveAsDcMotor.setPower(0);
back_left_driveAsDcMotor.setPower(0);
back_right_driveAsDcMotor.setPower(0);
limelightAsLimelight3A.stop();
}