Incredibots Academy

Now that you know how to write Subsystems and Commands, it's time to bring them together into your first TeleOp (Teleoperated) program!

In our Command-Based Architecture, we don't put all our code directly into the OpMode. Instead, we use a central Robot class to bind everything together.

The Robot Class (e.g., Incredibot.java): The central hub that instantiates subsystems and binds your gamepad buttons to commands.
The OpMode (e.g., MainTeleop.java): The actual program you select on the Driver Station to run the robot, which simply tells the Command Scheduler to run.

If you want to learn more about the physical robot hardware before coding it, check out the Electronics and Mechanical tracks!

Let's set up the essential files you need to get your robot moving.

Step 1: Create the OpMode Folder

You should already have the subsystems and commands packages from the previous lessons. Now we need the opmodes package.

Open your project in Android Studio.
In the Project pane on the left, navigate to TeamCode > src > main > java > org > firstinspires > ftc > teamcode.
Right-click the teamcode folder, select New > Package, and name it opmodes.
Right-click your new opmodes package, select New > Package, and name it teleop.

Step 2: Setting up a Mecanum Arcade Drive

Before we write the Robot class, let's look at how to set up the most common drivetrain in FTC: the Mecanum Drive. We will use FTCLib's built-in MecanumDrive class to make an "Arcade Drive" (left stick moves forward/back/strafe, right stick turns).

Here is a simplified DriveSubsystem.java that you would put in your subsystems folder:

package org.firstinspires.ftc.teamcode.subsystems;
 
import com.arcrobotics.ftclib.command.SubsystemBase;
import com.arcrobotics.ftclib.drivebase.MecanumDrive;
import com.arcrobotics.ftclib.hardware.motors.Motor;
import com.qualcomm.robotcore.hardware.HardwareMap;
 
public class DriveSubsystem extends SubsystemBase {
 
    // FTCLib provides a built-in MecanumDrive class that handles the math for us!
    private final MecanumDrive drive;
 
    public DriveSubsystem(HardwareMap hwMap) {
        // We initialize 4 FTCLib Motors. 
        // The strings must match your Driver Station configuration exactly.
        Motor frontLeft = new Motor(hwMap, "front_left");
        Motor frontRight = new Motor(hwMap, "front_right");
        Motor backLeft = new Motor(hwMap, "back_left");
        Motor backRight = new Motor(hwMap, "back_right");
 
        // We pass the 4 motors into the FTCLib MecanumDrive class
        drive = new MecanumDrive(frontLeft, frontRight, backLeft, backRight);
    }
 
    // This is our Action Method. It takes joystick inputs and passes them to the drive class.
    public void driveRobotCentric(double strafeSpeed, double forwardSpeed, double turnSpeed) {
        // driveRobotCentric calculates the power for all 4 wheels automatically!
        drive.driveRobotCentric(strafeSpeed, forwardSpeed, turnSpeed);
    }
}

Now, here is the DriveRobotCommand.java that goes in your commands folder. Notice how it takes in a GamepadEx (an FTCLib controller) to read the joysticks!

package org.firstinspires.ftc.teamcode.commands;
 
import com.arcrobotics.ftclib.command.CommandBase;
import com.arcrobotics.ftclib.gamepad.GamepadEx;
import org.firstinspires.ftc.teamcode.subsystems.DriveSubsystem;
 
public class DriveRobotCommand extends CommandBase {
 
    private final DriveSubsystem drive;
    private final GamepadEx driverGamepad;
 
    public DriveRobotCommand(DriveSubsystem drive, GamepadEx driverGamepad) {
        this.drive = drive;
        this.driverGamepad = driverGamepad;
        
        // Claim the drive subsystem so nothing else can drive the robot while this is active!
        addRequirements(this.drive);
    }
 
    @Override
    public void execute() {
        // We read the joystick values every frame.
        // Arcade Drive:
        // Left stick X = Strafe (Left/Right)
        // Left stick Y = Forward/Backward
        // Right stick X = Turn
        drive.driveRobotCentric(
                driverGamepad.getLeftX(),
                driverGamepad.getLeftY(),
                driverGamepad.getRightX()
        );
    }
}

Step 3: Create the Robot Class

The Robot class is the brains of the operation. It initializes your subsystems and binds your commands to the controller.

Right-click the teamcode folder and select New > Java Class.
Name it something that makes sense for your robot (for example, Incredibot or MyRobot).
Copy and paste the following skeleton code:

package org.firstinspires.ftc.teamcode;
 
import com.arcrobotics.ftclib.command.CommandScheduler;
import com.arcrobotics.ftclib.command.Robot;
import com.arcrobotics.ftclib.gamepad.GamepadEx;
import com.qualcomm.robotcore.hardware.HardwareMap;
 
// Import the subsystems and commands we wrote!
import org.firstinspires.ftc.teamcode.subsystems.DriveSubsystem;
import org.firstinspires.ftc.teamcode.commands.DriveRobotCommand;
 
public class Incredibot extends Robot {
    
    public enum OpModeType {
        TELEOP,
        AUTO
    }
 
    // 1. Declare our subsystems
    public final DriveSubsystem driveSubsystem;
 
    public Incredibot(HardwareMap hwMap) {
        // 2. Initialize our subsystems with the hardware map
        driveSubsystem = new DriveSubsystem(hwMap);
    }
 
    public void initialize(OpModeType opModeType, GamepadEx driverGamepad, GamepadEx operatorGamepad) {
        if (opModeType == OpModeType.TELEOP) {
            initTeleop(driverGamepad, operatorGamepad);
        } else if (opModeType == OpModeType.AUTO) {
            initAuto();
        }
    }
 
    public void initTeleop(GamepadEx driverGamepad, GamepadEx operatorGamepad) {
        CommandScheduler.getInstance().reset();
        
        // 3. SET DEFAULT COMMANDS
        // A "Default Command" runs automatically whenever no other command is using the subsystem.
        // Because we always want the robot to listen to the joysticks for driving,
        // we set DriveRobotCommand as the default command for the DriveSubsystem!
        driveSubsystem.setDefaultCommand(new DriveRobotCommand(driveSubsystem, driverGamepad));
    }
 
    public void initAuto() {
        CommandScheduler.getInstance().reset();
    }
}

Step 4: Create the TeleOp OpMode

An OpMode is the script that actually appears on your Driver Station phone for you to select and run.

Right-click the teleop package you created earlier (teamcode > opmodes > teleop) and select New > Java Class.
Name it MainTeleop.
Copy and paste the following code:

package org.firstinspires.ftc.teamcode.opmodes.teleop;
 
import com.arcrobotics.ftclib.command.CommandScheduler;
import com.arcrobotics.ftclib.gamepad.GamepadEx;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
// Import your robot class!
import org.firstinspires.ftc.teamcode.Incredibot;
 
// The @TeleOp annotation makes this visible on the Driver Station phone!
@TeleOp(name = "MainTeleop", group = "TeleOp")
public class MainTeleop extends LinearOpMode {
    
    private Incredibot incredibot;
    private GamepadEx driverGamepad;
    private GamepadEx operatorGamepad;
 
    @Override
    public void runOpMode() throws InterruptedException {
        // =========================================================
        // 1. INITIALIZATION PHASE (When you press 'INIT' on the phone)
        // =========================================================
        
        // Wrap the standard FTC gamepads in FTCLib's GamepadEx class
        // This gives us access to advanced button bindings!
        driverGamepad = new GamepadEx(gamepad1);
        operatorGamepad = new GamepadEx(gamepad2);
 
        // Initialize the robot class we just wrote, passing in the hardware map
        incredibot = new Incredibot(hardwareMap);
 
        // =========================================================
        // 2. WAIT FOR START 
        // =========================================================
        // The code pauses here until you press the 'Play' button on the phone.
        waitForStart();
 
        // =========================================================
        // 3. START PHASE (Runs ONCE when you press 'Play')
        // =========================================================
        // Tell our Robot class to run its TeleOp setup method (which sets up the default commands)
        incredibot.initialize(Incredibot.OpModeType.TELEOP, driverGamepad, operatorGamepad);
 
        // =========================================================
        // 4. MAIN TELEOP LOOP
        // =========================================================
        // This loop runs continuously until you press 'Stop' on the phone.
        while (opModeIsActive() && !isStopRequested()) {
            
            // THE MOST IMPORTANT LINE IN COMMAND-BASED PROGRAMMING:
            // This single line tells the scheduler to poll the gamepad buttons,
            // execute the active commands (like our Drive command), 
            // and manage the subsystems!
            CommandScheduler.getInstance().run();
            
            telemetry.update();
        }
 
        // =========================================================
        // 5. CLEANUP PHASE (When you press 'Stop')
        // =========================================================
        // Flush the scheduler so it's clean for the next time you run a program.
        CommandScheduler.getInstance().reset();
    }
}

Check Your Understanding

Check yourself

Your First TeleOp