Command Based Programming

As our robot programs become more complex, a framework is needed to help keep related functionality together (cohesion) while preventing unrelated functions from becoming entangled (coupling). Improving cohesion and reducing coupling are key concepts in software engineering. WPILib provides a framework called Command Based Programming that helps with this. You can read about it in ​ScreenStepsLive.

In command-based programming, we define

• subsystems representing physical parts of our robot: intakes, shooters, turrets, drivetrains, etc.
• commands representing activities that can be done by one or more subsystem (e.g. throwing a frisbee)
• command groups represent behaviors: a set of activities that must be performed in a particular sequence (such as pointing a turret at a target, adjusting the elevation angle, and then throwing a frisbee into it).

A given activity may require more than one subsystem (e.g. throwing a frisbee might require both the turret and the shooter). More than one activity can be running at the same time (e.g. the robot might be driving while throwing frisbees). The Command-Based framework helps organize all of this activity so that multiple things can be happening at the same time without interfering with each other (e.g. preventing two commands from trying to control of the same subsystem at the same time).

Creating a Command-Based Program

Create a new project in VSCode (WPILib Icon->Create a new project->Template->Java->Command Robot). Notice that in the project src folder, addition to the Main.java and Robot.java we're used to seeing, the project also has:

• OI.java - where the Operator Interface is defined
• RobotMap.java - where global constants are defined (e.g. motor controller addresses)
• commands folder - where your Commands will be stored
• subsystems folder - where your Subsystems will be stored

Robot.java still extends TimedRobot (the framework we've been using until now) and the methods robotInit(), robotPeriodic(), autonomousInit(), autonomousPeriodic(), teleopInit(), teleopPeriodic(). However, it now includes an example subsystem and an instance of the OI (Operator Interface) that represents the driver station; the OI helps bind the controls on the driver station (e.g. joystick, xbox controller) to the activities (commands, command groups) that your robot will be performing. For example, the OI allows you to connect a button on your controller to an activity like launching a frisbee.

Example Code

ScreenStepsLive has a very good example of how a program might look if programmed for TimedRobot and then the same program converted to CommandRobot; you should review it ​here

The following is an example of a very simple Command Based Robot that has a single subsystem consisting of a servo motor with two positions: Open, Closed and they are selected using the A and B buttons on a gamepad. The servo motor should be connected to PWM port 0 and the code uses a Logitech F310 for the operator interface. Replace the template files with:

RobotMap.java:

package frc.robot;
public class RobotMap {
  public static int SERVO_PORT = 0;   // PWM port for servo motor
  public static int OPEN_BUTTON = 1;  // A button opens mechanism
  public static int CLOSE_BUTTON = 2; // B button closes mechanism
}

Add ServoSubsystem.java under subsystems:

package frc.robot.subsystems;

import edu.wpi.first.wpilibj.command.Subsystem;
import frc.robot.RobotMap;
import edu.wpi.first.wpilibj.Servo;

public class ServoSubsystem extends Subsystem {
  private Servo servo;

  private static ServoSubsystem instance = null;

  private ServoSubsystem() {
      servo = new Servo(RobotMap.SERVO_PORT);
  }

  // Singleton design pattern
  public static ServoSubsystem getInstance() {
      if (instance == null) {
          instance = new ServoSubsystem();
      }
      return instance;
  }

  public void setAngle(double degrees) {
    servo.setAngle(degrees);
  }

  @Override
  public void initDefaultCommand() {
    // Set the default command for a subsystem here.
    // setDefaultCommand(new MySpecialCommand());
  }
}

Add OpenCommand.java and CloseCommand.java under commands: OpenCommand.java:

package frc.robot.commands;

import edu.wpi.first.wpilibj.command.Command;
import frc.robot.Robot;

public class OpenCommand extends Command {
  public OpenCommand() {
    requires(Robot.m_servo);
  }

  @Override
  protected boolean isFinished() {
    System.out.println("Open Pressed.");
    Robot.m_servo.setAngle(30); // set servo to open position
    return true;
  }
}

CloseCommand.java:

package frc.robot.commands;

import edu.wpi.first.wpilibj.command.Command;
import frc.robot.Robot;

public class CloseCommand extends Command {
  public CloseCommand() {
    requires(Robot.m_servo);
  }

  @Override
  protected boolean isFinished() {
    System.out.println("Close Pressed.");
    Robot.m_servo.setAngle(120); // set servo to closed position
    return true;
  }
}

OI.java:

package frc.robot;

import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.buttons.Button;
import edu.wpi.first.wpilibj.buttons.JoystickButton;
import frc.robot.RobotMap;
import frc.robot.commands.*;

public class OI {
  XboxController xbox = new XboxController(0);
  Button openButton = new JoystickButton(xbox, RobotMap.OPEN_BUTTON);
  Button closeButton = new JoystickButton(xbox, RobotMap.CLOSE_BUTTON);

  public OI() {
    openButton.whenPressed(new OpenCommand());
    closeButton.whenPressed(new CloseCommand());
  }
}

Robot.java:

package frc.robot;

import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.command.Scheduler;
import frc.robot.subsystems.ServoSubsystem;

public class Robot extends TimedRobot {
  public static ServoSubsystem m_servo = ServoSubsystem.getInstance();
  public static OI m_oi;

  @Override
  public void robotInit() {
    m_oi = new OI();
  }

  @Override
  public void disabledPeriodic() {
    Scheduler.getInstance().run();
  }

  /**
   * This function is called periodically during autonomous.
   */
  @Override
  public void autonomousPeriodic() {
    Scheduler.getInstance().run();
  }

  @Override
  public void teleopPeriodic() {
    Scheduler.getInstance().run();
  }
}

How it works

Command-based programming is built on top of the IterativeRobot template. In each of the template's periodic() methods, Scheduler.getInstance().run() is called. This takes all currently running Commands -- which behave like threads, but can't include waits like typical threads because it's a type of threading known as "cooperative" multitasking -- and runs them, first initializing any new ones and checking whether each one has completed yet. It's useful because these Commands can be easily joined together in CommandGroups? to make autonomous modes, or can be mapped to buttons to easily allow complex teleop actions, etc.

• ​Intro to Command Based Programming