Need help understanding command-based Robots

Before the new season starts, I’d like to learn command-based programming. Last year, I used the sample robot. Any sites besides screensteps that I could use? I am confused about the subsystems, commands, and how the drivetrain is made.

Command: Basically some action that you can define. This might be an autonomous mode action, or a teleop action, or something you use in both modes. initialize() sets up the action to happen, then execute() is repeatedly called until isFinished() is true, then end() is called to clean up.

public class MoveToPositionCommand extends Command {
    private int encoderTicksToDrive;
    public MoveToPositionCommand(int encoderTicksToDrive) {
        this.encoderTicksToDrive = encoderTicksToDrive;
    public void initialize() {

    public void execute() {
        int leftEncoder = driveSubsystem.getLeftEncoder();
        int rightEncoder = driveSubsystem.getRightEncoder();
        double turn = DriveSubsystem.SOME_TURN_CONSTANT * (rightEncoder - leftEncoder);
        double throttle = DriveSubsystem.AUTO_DRIVE_SPEED;, turn);

    public boolean isFinished() {
        return driveSubsystem.getLeftEncoder() > encoderTicksToDrive || driveSubsystem.getRightEncoder() > encoderTicksToDrive;

    public void end() {, 0);

Subsystem: Exactly what it sounds like. A virtual representation of a robot subsystem. Subsystems have WPILib objects and define methods that commands can use

public class DriveSubsystem {
    public static final double SOME_CONSTANT = 42;

    CANTalon leftDrive, rightDrive;
    Encoder leftEncoder, rightEncoder;

    public DriveSubsystem() {
        // initialize objects

    public void drive(double throttle, double turn) {
        // do the driving

    // other methods that commands need, etc

Edit: oops I just realized I posted Java in the C++ forum. It shouldn’t be too difficult to translate though :slight_smile:

Have you checked out the robotbuilder? If not it provide a very concrete way to understand how subsystems, commands and OI all fit together. It generates readable code based on your description.

Think of it this way, your robot is composed of one or more subsystems. Each subsystem will have one or more commands associated with it.

You will almost always have a subsystem for your drivetrain, which at least initially will probably use either the 2 or 4 motor controller. You will then create a teleop_drive command for your drivetrain controller.

Generate the code, and fill in a few blanks and you should be able to get your robot moving pretty quickly. Give that a try and if you get stuck let us know.

Ok, thanks a lot! And if I want to execute a command, is it “MoveToPosition.execute();”? (without “”)

Nope, just run MoveToPosition(); from a CommandGroup or assign it to a button in OI and it’ll go.

You would generally associate the MoveToPosition command with operator interface (OI). Using robot builder you can assign it to when a button on a joystick (or gamepad) is pressed, held, or released.

The code ends up looking something like (but again, let robot builder do the lifting until you get the hang of it):

    // where the coPilot and cmdMove2Pos variables are generally 
    // defined in your OI.h file like
    std::shared_ptr<Joystick> coPilot;
    std::shared_ptr<JoystickButton> cmdMove2Pos;

    // in you OI.cpp file in the constructor
    coPilot.reset(new Joystick(1));
    coPilot.cmdMove2Pos.reset(new JoystickButton(coPilot.get(), 2));
    cmdMove2Pos->WhenPressed(new MoveToPosition());

You can also setup a command (or different commands based on a dashboard option) to run as your auton. The commands can run in sequence, so you can build complicated autonomous behavior from simpler command options.

To clarify (as this does NOT make much sense until you wrap your head around it, especially if you’re coming from a sampleRobot mindset): You just create the commands with new MoveToPosition() and pass them to the buttons, joysticks, or other control objects. The internals of those control objects call your initialize(), execute(), isFinished(), and end() methods at the appropriate times. You should only put “one pass” of your execute loop in the execute command; it will be called repeatedly. Any information you need to carry from one call to execute() to another should be declared as part of the object and instantiated/initialized in initialize(). Each of your functions (especially execute() and isFinished()) should be written to return as quickly as possible - do not put any waits or loops of more than a very few passes in them.

After being confused at first about command-based programming, I am now a big fan of it. It allows for some pretty complex movements with organized and reusable code. The one definite downside is that it can actually make the code seem more complex than it actually is to someone who hasn’t been working on it because of all the extra file structure and such. There’s some other downsides to it if you want to really squeeze every bit of performance out of the roboRIO possible - but 5414 isn’t at that point yet. euhlmann covered it and screensteps does a pretty good job in my opinion, but I’ll just kind of repeat and add a little bit:

I like to start with the RobotMap - adding in static variable names for whatever motors or buttons you plan on having. For instance you might have:

public class RobotMap {
	public static int CollectorRetract = 12;
	public static int CollectorExtend = 8;	


and then in OI, making the connection from your interface to your new functions you’ll be adding:

public class OI {

	private Joystick stick = new Joystick(0);		//Joystick in Driver Station Spot 0
	private Joystick stick2 = new Joystick(1);		//Joystick in Driver Station Spot 1
	public OI()
		//Stick 2 Controls
		JoystickButton CollectorInStick2 = new JoystickButton(stick2, RobotMap.CollectorRetract);
		JoystickButton CollectorOutStick2 = new JoystickButton(stick2, RobotMap.CollectorExtend);

		CollectorInStick2.whileHeld(new CollectorInCommand());			//Collector Retract
		CollectorOutStick2.whileHeld(new CollectorOutCommand());			//Collector Extend
		CollectorInStick2.whenReleased(new CollectorStopCommand());		//Collector Retract Stop
		CollectorOutStick2.whenReleased(new CollectorStopCommand());		//Collector Extend Stop

then, if it’s not defined already, you’ll have to define your subsystem. Your subsystem should include any motor controllers or sensors for that group of thing. For a drivetrain, that could be 4-6 CIMs, 2 encoders, a gyro, servos, shifting pneumatics, etc (but start small then add the rest in). For this simple example I’m walking through, it’s just one motor. In this, we have three different things we do with our collector: extent, retract, and stop. For something (potentially) more complicated like a drivetrain, you might have functions to get encoder or gyro data, extend or retract cylinders to change gears, drive in arcade mode/tank mode, drive in a slow/demo mode that sets power to 75%, etc.

public class Collector extends Subsystem {
	private SpeedController deploy;
	// Put methods for controlling this subsystem
    // here. Call these from Commands.

    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
        //setDefaultCommand(new MySpecialCommand());
    public Collector() {
    	deploy = new Spark(4);
    public void CollectorIn(){
    public void CollectorOut(){
    public void CollectorStop(){

Then you have your command files that then use the functions within the subsystems. From what we set up in the file, we’d also want to create and You call the functions that exist within your subsystem from your commands.

public class CollectorIn extends Command {

    public CollectorInCommand() {

    // Called just before this Command runs the first time
    protected void initialize() {

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
    	return false; //We set this up in the OI so that it would call a function to stop it when the button was released

    // Called once after isFinished returns true
    protected void end() {

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {

You can also make the command groups as discussed on WPI lib. It’s simple enough to understand that I won’t include an example here. The great thing about using command based programming is that if you make a change to say, how fast you want the collector to extend - it takes affect anywhere you use that command. With sample robot - if you have the collector extend in autonomous and from a button push in tele-op, you have to be careful to make sure and go change it in both places. You can also include variable in it, so you can have a command that say turns your robot X degrees, with a different parameter sent that reuses the rest of the code the same.

Hope this helped some.

I have my programmers watch these 6 videos. We are a Java team but these videos do apply to C++ as well. Last year was my first year as a coach, I watched these, Played along and simulated the programs in FRCSim. The one student program we had last year called it “cheating”.

Wow this thread is really useful for beginners such as myself good job guys.I was also having trouble understanding the subsystems as well as how we were to assign the controller to the code onto the robot but this thread cleared a lot of those problems up.Thanks Guys! :slight_smile:

Glad i found this thread… thank you all!!!

We are planning on driving a 6 CIM motor using talon SRX… can i build it with Robot builder using the 4 motor system and then later on add the code needed to add the extra 2 motors?

I don’t think so. What I’m thinking is use the 4 motor system in robot builder, enter in your 4 motors. Then also include a 2 motor system. Though that might be what you mean.

If you can, you could write it manually.

We have an example that might help…

… basically slave the fifth and sixth talon outside of RobotDrive to the appropriate talons inside RobotDrive.

It’s java but should be easy to port to C++.

Thank you !

Being a newbie with Java…this code is added to the class… is there anything else in other class i need to add?