Week 5: The Command-Based Framework + Deploying to, and Running on, the Robot

This week marks a major milestone: you'll start writing code that can actually control a robot! We'll bridge the gap between pure Java and FRC programming by introducing the WPILib library and the fundamental concepts of the Command-Based programming model. This is a system for organizing your robot code into interacting Subsystems and Commands, making it easier to write, manage, and debug complex robot behaviors.

How do we actually run code on the robot?

It usually comes down to these steps:

• Write code on your computer, starting from a template. Use WPILib!

• Use the FRC Driver Station to connect to the robot, often by connecting to the roboRIO's wifi network through your computer's settings.

• Use the WPILib "Build Robot Code" or "Deploy Robot Code" commands in VSCode to build and deploy the code to the robot.

• Use the FRC Driver Station to run the code on the robot. Click Enable once the code is deployed!

Goals for this week:

• Learn about useful WPILib utilities like Units and Geometry classes.

• Understand the Command-Based structure: Subsystems, Commands, and RobotContainer.

• Understand the TimedRobot lifecycle.

• Write a Drivetrain subsystem.

• Understand and use Commands to control the subsystem.

• Learn how to bind commands to controller buttons using Triggers.

• Understand how to create sequential and parallel command sequences.

1. WPILib Utilities

Before diving into the robot structure, let's explore some helpful tools WPILib provides.

Typesafe Units

A common source of bugs in physics and robotics code is mixing up units (e.g., using meters where you meant to use feet). WPILib provides a set of "typesafe" unit classes that make this kind of error impossible at compile time.

Different types of units like distances, angles, and velocities are represented by different classes.

import edu.wpi.first.math.measure.Angle;

import edu.wpi.first.math.measure.Distance;

import edu.wpi.first.math.measure.Measure;

import edu.wpi.first.math.measure.Velocity;

import edu.wpi.first.math.util.Units;

// 10 feet

Measure<Distance> distance = Units.Feet.of(10.0); 

// 90 degrees

Measure<Angle> angle = Units.Degrees.of(90.0);

// 5 feet per second

Measure<Velocity<Distance>> velocity = Units.FeetPerSecond.of(5.0);

All units are a form of Measure with a specified type. This makes it easy to work with different units and convert between them safely. You can learn more in the WPILib documentation.

Geometry Classes

WPILib also provides classes to represent points, rotations, and poses in 2D and 3D space, which integrate with the typesafe units. These are essential for robot kinematics and field positioning.

Class

Description

Translation2d

Represents a 2D point with x and y coordinates.

Rotation2d

Represents a 2D rotation using a point on the unit circle

Pose2d

Represents a 2D pose with a translation and rotation.

Translation3d

Represents a 3D point with x, y, and z coordinates.

Rotation3d

Represents a 3D rotation using a quaternion.

Pose3d

Represents a 3D pose with a translation and rotation.

You can learn more about these classes in the WPILib documentation.

Math Utilities

WPILib also includes a Units class with static methods for converting between different units stored as doubles (e.g., Units.feetToMeters(10.0)). While useful, you should prefer the typesafe Measure classes whenever possible to prevent errors.

2. Anatomy of an FRC Project

When you create a new FRC project, you'll see a lot of files and folders. Here are the most important ones:

• build.gradle: This file tells our build system, Gradle, how to compile and package our code. You'll rarely need to touch this, but it's good to know what it is.

• src/main/java/frc/robot/: This is where all of our Java code will live.

  • Main.java: The entry point of our program. You'll never need to change this file.

  • Robot.java: The main class for our robot. It controls the overall flow of the program and runs the Command Scheduler.

  • RobotContainer.java: This class is responsible for creating our subsystems, commands, and controller bindings. This is where we'll spend a lot of our time.

  • subsystems/: A folder where we'll put our Subsystem classes.

  • commands/: A folder where we'll put our Command classes.

3. The Robot Class and Lifecycle

Our Robot.java class extends TimedRobot. This gives it methods that are called periodically for each robot mode (disabled, autonomous, teleop). All periodic methods are run every 20ms (50 times a second) by default. This is our "loop time".

• robotInit(): Called once when the robot is first turned on. Use this for one-time setup.

• robotPeriodic(): Called every 20ms, regardless of the robot's mode. The Command Scheduler's run() method is called here automatically.

• disabledInit()/disabledPeriodic(): Called when the robot enters/is in disabled mode.

• autonomousInit()/autonomousPeriodic(): Called at the start of/during the autonomous period.

• teleopInit()/teleopPeriodic(): Called at the start of/during the teleoperated period.

The Robot class is the main class of your robot code. It's responsible for initializing everything and running the Command Scheduler, which is the part of WPILib that actually tracks and runs commands.

4. Subsystems: Organizing Your Robot

A subsystem is a class that represents a single mechanism on the robot (e.g., drivetrain, arm, intake). It encapsulates all the hardware (motors, sensors) and logic for that mechanism. Subsystems are intended to store the state of a mechanism and control access to the hardware, ensuring it's only given one instruction at a time.

A subsystem class typically contains:

• Hardware Objects: Fields for the motors, sensors, etc. that control the subsystem. These should usually be private final.

• Control Methods: public methods that command the hardware (e.g., drive(speed, rotation)). These methods are used by Commands.

• Periodic Logic: The periodic() method (which you must @Override) is called every robot loop by the scheduler. You can use it for things like updating sensor readings or sending data to a dashboard.

• Command Factories (Optional but recommended): public methods that create pre-configured Command objects for common actions.

Let's create a Drivetrain subsystem. Create a new file Drivetrain.java in a new subsystems folder (src/main/java/frc/robot/subsystems/).

package frc.robot.subsystems;

import edu.wpi.first.wpilibj.drive.DifferentialDrive;

import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;

import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;

import edu.wpi.first.wpilibj2.command.Command;

import edu.wpi.first.wpilibj2.command.SubsystemBase;

import java.util.function.DoubleSupplier;

public class Drivetrain extends SubsystemBase {

    // Hardware objects

    private final PWMSparkMax leftFront = new PWMSparkMax(0);

    private final PWMSparkMax leftRear = new PWMSparkMax(1);

    private final PWMSparkMax rightFront = new PWMSparkMax(2);

    private final PWMSparkMax rightRear = new PWMSparkMax(3);

    private final MotorControllerGroup leftMotors = new MotorControllerGroup(leftFront, leftRear);

    private final MotorControllerGroup rightMotors = new MotorControllerGroup(rightFront, rightRear);

    private final DifferentialDrive differentialDrive = new DifferentialDrive(leftMotors, rightMotors);

    public Drivetrain() {

        // Invert the right side motors because they are mounted backwards

        rightMotors.setInverted(true);

    }

    // Control method

    public void arcadeDrive(double fwd, double rot) {

        differentialDrive.arcadeDrive(fwd, rot);

    }

    @Override

    public void periodic() {

        // This method will be called once per scheduler run

    }

    // Command factory

    public Command arcadeDriveCommand(DoubleSupplier fwdSupplier, DoubleSupplier rotSupplier) {

        // The run method is a factory for a command that runs the given code continuously.

        // It requires the subsystem, so no other command can run on it at the same time.

        return run(() -> arcadeDrive(fwdSupplier.getAsDouble(), rotSupplier.getAsDouble()));

    }

}

5. Suppliers

You may have noticed the DoubleSupplier type. When you pass a double to a method, you pass its value at that moment. But what about joystick values, which are constantly changing?

A Supplier stores an action that can be called to get a value. When you call the .get() (or .getAsDouble() for DoubleSupplier) method on the supplier, it executes that action and returns the current value. We use suppliers (often as lambda expressions like () -> joystick.getY()) to give commands access to the latest joystick values each time the command's loop runs.

// joystick is a Joystick object

// This creates a supplier that, when called, will get the current Y-axis value.

DoubleSupplier axisSupplier = () -> joystick.getRawAxis(0);

// Later, in a command's execute() loop:

System.out.println(axisSupplier.getAsDouble()); // Prints the *current* value of the joystick axis

6. Commands

Commands contain the logic to control a subsystem. The key advantage of commands is that they are managed by the scheduler, which prevents you from controlling a subsystem from multiple places at once. This avoids bugs and unsafe robot behavior.

Commands can be created in many ways. The easiest way is using the factory methods provided by SubsystemBase or Commands.

Method

Description

run(action)

Runs the action lambda every robot loop until the command is cancelled.

runOnce(action)

Runs the action lambda one time when the command is scheduled.

runEnd(run, end)

Runs the run lambda every loop, and the end lambda one time when it ends/is interrupted.

startEnd(start, end)

Runs the start lambda one time when scheduled, and the end lambda one time when it ends.

Commands.print(msg)

A pre-built command to print a message to the console.

Commands.waitSeconds(sec)

A pre-built command that does nothing, but finishes after a set time.

In our Drivetrain example, the arcadeDriveCommand method is a command factory. It uses the run method to create a command that is associated with the subsystem (this) and will call our arcadeDrive method every 20ms.

// This is the command factory from our subsystem

public Command arcadeDriveCommand(DoubleSupplier fwdSupplier, DoubleSupplier rotSupplier) {

    return run(() -> arcadeDrive(fwdSupplier.getAsDouble(), rotSupplier.getAsDouble()));

}

7. RobotContainer: Tying it all together

The RobotContainer is where we instantiate our subsystems and controllers, and set up the bindings between them.

package frc.robot;

import edu.wpi.first.wpilibj.XboxController;

import edu.wpi.first.wpilibj2.command.button.CommandXboxController;

import frc.robot.subsystems.Drivetrain;

public class RobotContainer {

    // Create an instance of the Drivetrain and other subsystems

    private final Drivetrain drivetrain = new Drivetrain();

    // Create an instance of the controller

    private final CommandXboxController driverController = new CommandXboxController(0);

    public RobotContainer() {

        // Configure the default command for the drivetrain.

        // This command will run whenever no other command is scheduled for the Drivetrain.

        drivetrain.setDefaultCommand(

            drivetrain.arcadeDriveCommand(

                () -> -driverController.getLeftY(),

                () -> driverController.getRightX()

            )

        );

        // Configure other button bindings

        configureBindings();

    }

    private void configureBindings() {

        // Other bindings go here

    }

}

Notice we set a default command. This is a command that runs automatically whenever no other command is using its required subsystem. Here, we use our command factory to create a drive command that is always running, using the joystick axes as suppliers.

8. Scheduling Commands with Triggers

How do we run commands that aren't the default? We use Triggers. A Trigger is essentially a "fancy boolean supplier" that represents a condition, like a button being pressed. We can then bind commands to activate when the trigger's condition becomes true.

The CommandXboxController and CommandJoystick classes are very helpful here, as they have methods that return Trigger objects directly for each button.

// In RobotContainer.java

private void configureBindings() {

    // Run a command once when the 'A' button is pressed

    driverController.a().onTrue(Commands.print("A button pressed!"));

    // Run a command continuously while the 'B' button is held

    // and stop it when 'B' is released.

    Command driveForward = drivetrain.arcadeDriveCommand(() -> 0.5, () -> 0.0);

    driverController.b().whileTrue(driveForward);

    // Toggle a command on and off each time the 'X' button is pressed.

    Command spinCommand = drivetrain.arcadeDriveCommand(() -> 0.0, () -> 0.5);

    driverController.x().toggleOnTrue(spinCommand);

}

There are many ways to bind commands to triggers. The three most common are:

• onTrue(command): Starts the command when the trigger becomes true. The command runs until it finishes on its own.

• whileTrue(command): Starts the command when the trigger becomes true, and cancels it when the trigger becomes false.

• toggleOnTrue(command): Starts the command when the trigger becomes true. If the trigger becomes true again, it cancels the command.

Safety when testing: Keep the drivetrain wheels off the ground or the robot on blocks when first running commands. Keep the Driver Station ready to Disable.

9. Command Groups

For more complex actions, you can combine commands into Command Groups. The most common types are sequential (one after another) and parallel (all at the same time).

// In RobotContainer.java, maybe in a method that returns a Command

// An autonomous routine for example

// Create some basic commands

Command driveForward = drivetrain.arcadeDriveCommand(() -> 0.5, () -> 0.0).withTimeout(2.0); // drive for 2s

Command turnRight = drivetrain.arcadeDriveCommand(() -> 0.0, () -> 0.5).withTimeout(1.0); // turn for 1s

Command stop = drivetrain.arcadeDriveCommand(() -> 0.0, () -> 0.0).withTimeout(0.1);

// Create a sequential command group

// This will drive, then turn, then drive again, then stop.

Command autoRoutine = driveForward.andThen(turnRight, driveForward, stop);

// Example of a parallel group.

// This would run an intake motor at the same time as driving forward.

// Command runIntake = intake.runIntakeCommand();

// Command driveAndIntake = driveForward.alongWith(runIntake);

What's Next?

Command-Based programming is a powerful way to structure your robot code. We've only scratched the surface. To learn more, you should always check out the WPILib documentation.

Resources

• WPILib Command-Based Programming Docs

• TimedRobot and the Robot Lifecycle

• Subsystems

• Commands

• RobotContainer

• Default Commands

• Binding Commands to Triggers

How to make Commands

• The two main types of Commands we use are runs and runOnces

  • run runs an action over and over until it is interrupted

  • runOnce runs once and then immediately stops

For example:

// Runs the intake rollers forever

Commands.run(intakeSubsystem::spinRoller, intakeSubsystem);

// Retracts the intake

// Note that the real hardware doesn't move instantly

// But we only need to set it once in code

Commands.runOnce(intakeSubsystem::retract, intakeSubsystem);

Currently, we tend to define these Commands in the Subsystem file, so we can make them even simpler by calling run and runOnce on the subsystem itself

// Inside IntakeSubsystem.java

// Notice how we don't need to define the requirements for these

// The subsystem does it implicitly

this.run(this::spinRoller);

this.runOnce(this::retract);

So we made a Command, now what?

• Commands get bound to Triggers

• Usually this is a button on the joystick

// In RobotContainer.java

// When the a button on the controller is pressed, run the rollers on the intake

driverController.a().onTrue(Commands.run(intakeSubsystem::runRollers, intakeSubsystem));

• This is somewhat wordy

• But Commands are objects, so we can pass them around!

• Let's make a method that returns the Command instead of having to make it here

// In IntakeSubsystem.java

public Command runRollersCommand() {

    // Note implicit requirements

    return this.run(this::runRollers);

}

Then we can replace the call in RobotContainer with this new method

// In RobotContainer.java

// When the a button on the controller is pressed, run the rollers on the intake

driverController.a().onTrue(intakeSubsystem.runRollersCommand());

Composing Commands

• These actions on their own are useful, but they only form simple, single actions

• Commands can be chained into Command groups

intakeSubsystem.extendCommand().andThen(intakeSubsystem.runRollersCommand())

• Several different "decorators" exist

  • until() will run and Command and then stop it when a condition is met

  • andThen() adds another Command after one finishes

  • alongWith() will run two or more Commands side by side until both finish

  • raceWith() will run two or more Commands side by side and interrupt all of them when any Command finishes

  • withTimeout() will interrupt a Command after a set amount of seconds

// An example of a more complex group

intakeSubsystem.extend().andThen(

    intakeSubsystem.runRollers().until(intakeSubsystem::hasGamePiece),

    intakeSubsystem.retract()

)

// Another example

elevatorSubsystem.runToScoringHeight()

  .alongWith(

    grabberSubsystem.holdGamePiece()

  ).until(elevatorSubsystem::isAtScoringHeight)

  .andThen(

    // Outtake game piece for 1 second

    grabberSubsystem.outtakeGamePiece().withTimeout(1.0)

  )