Write a Full Command-Based TeleOp

Capstone TeleOp that composes drive, mechanisms, and safety.

Module 5: TeleOp and Driver ControlCapstone

In this lesson, you will:

01Combine drivetrain and mechanism controls.
02Schedule reusable commands.
03Keep the loop readable.

Concept narrative

A full TeleOp should be boring to read. The main loop updates gamepad snapshots, runs the scheduler, handles drive, schedules mechanism commands, updates subsystems, and reports only useful telemetry.

This lesson should be read as a robotics lesson first and a programming lesson second. The code matters because it lets the team create repeatable behavior under match pressure. Students should slow down long enough to name the inputs, outputs, assumptions, and safety limits before they touch the robot.

Robot mental model

The robot is now a set of capabilities rather than scattered motor writes. Drive code owns movement, subsystems own mechanisms, commands request behavior, and safe state coordinates recovery.

A good mental model gives the team a shared language. When a driver, builder, and programmer can point to the same behavior and use the same words, debugging gets calmer and code review becomes useful instead of personal.

Implementation walkthrough

Start from the working robot-centric or field-centric drive. Add command scheduler, then one mechanism command, then safe state, then driver telemetry. Do not add every mechanism at once.

Keep the implementation staged. First create the smallest version that compiles. Then add telemetry that proves it is running. Then connect one hardware device or one decision. Finally, repeat the test from a cold init so the team knows it was not a lucky hot reload.

MainTeleOp.javaJava

public void run() {
    previous.copy(current);
    current.copy(gamepad2);
    CommandScheduler.getInstance().run();
    driveWithGamepad(gamepad1);
    if (current.a && !previous.a) scheduler.schedule(new ArmToScore());
    robot.periodic();
    telemetry.addData("state", robot.summary());
    telemetry.update();
}

Common mistakes and debugging

If commands fight, inspect subsystem ownership. If the loop becomes unreadable, extract helpers. If telemetry is noisy, separate programmer telemetry from driver telemetry.

Use the five-value debugging habit: input, state, target, measurement, output. If one of those values is missing, add it before rewriting logic. The goal is to make the robot tell the truth about what it thinks is happening.

Checkpoint

Scheduler runs every loop.
Drive and mechanisms are separated.
Safe state is tested under driver control.

Reflection check

Check your understanding before moving on.

What is the most important habit in Write a Full Command-Based TeleOp?

0 of 1 answered

References

FIRST FTC DocsOfficial SDK, Robot Controller, and programming reference.Game Manual 0FTC community reference for programming, controls, and robot design.