Commands - hammerheads5000/Team-Documentation GitHub Wiki

See WPILib's Documentation on Commands. Commands are the main form of interaction between RobotContainer.java and the subsystems, meaning that usually commands are assigned to triggers so that when a trigger occurs, the command will be executed. A command should be doing just one thing, such as raising an arm, grabbing something, etc.

The basic structure of a command is as follows:

package frc.robot.commands;

import edu.wpi.first.wpilibj2.command.CommandBase;
import frc.robot.subsystems.ExampleSubsystem;

public class SomeCommand extends CommandBase {
  // Put instance variables here, these are what you need to call from the command to execute actions
  ExampleSubsystem exampleSubsystem; // Declare, but do not initialize
  
  /** Creates a new SomeCommand. */
  public SomeCommand(/* Put arguments in here, these should correspond to instance variables */ ExampleSubsystem exampleSubsystem) {
    // Set instance variables to arguments like so
    this.exampleSubsystem = exampleSubsystem;
  }

  // Called once when the command is initially scheduled.
  @Override
  public void initialize() {}

  // Called every time the scheduler runs while the command is scheduled.
  @Override
  public void execute() {
    // This is where most your code will be, IT WILL BE CALLED REPEATEDLY
    exampleSubsystem.doSomething();
  }

  // Called once the command ends or is interrupted.
  @Override
  public void end(boolean interrupted) {}

  // Returns true when the command should end.
  @Override
  public boolean isFinished() {
    return false;
  }
}

You would use this structure if you want the command to run for a period of time, often used as an argument to whileTrue called on a trigger

If you just need something to happen once, use an InstantCommand:

package frc.robot.commands;

import edu.wpi.first.wpilibj2.command.InstantCommand;
import frc.robot.subsystems.ExampleSubsystem;

public class ExampleInstantCommand extends InstantCommand {
  // Instance variables (do not initialize)
  ExampleSubsystem exampleSubsystem;
  
  public ExampleInstantCommand(ExampleSubsystem exampleSubsystem) {
    this.exampleSubsystem = exampleSubsystem;
  }

  // Called when the command is initially scheduled.
  @Override
  public void initialize() {
    // This is where you would put the main body of code to do things
    exampleSubsystem.doSomething();
  }
}

It is good practice to create a new file for more complex commands, but for very simple commands you can create them inline (inside another file, mainly RobotContainer.java)

Inline Commands

// Pass these as arguments to something. Takes a lambda and subsystem dependencies as arguments:

// Runs repeatedly until interrupted
new RunCommand(() -> someFunction(stuff), subsystemRequirements);

// Runs once
new InstantCommand(() -> someMethod(stuff), subsystemRequirements);

// Runs something at start and another at end:
new StartEndCommand(() -> doSomethingAtStart(), () -> doSomethingAtEnd(), subsystemReqs);

There are other ways to create commands, but for the most part, we do not use them.

For more information on lambdas, see W3Schools' page on lambdas.

Examples

Raising an Arm

To create a command for raising an arm, say that we have subsystem LowerArmSubsystem.java with methods raise(), accountForUpperArmMovement(upperArmSubsystem), and lock(), and UpperArmSubsystem.java with method setPosition(positionInDegrees) and reachedPosition() which returns a boolean. The command might look like this:

package frc.robot.commands;

import edu.wpi.first.wpilibj2.command.CommandBase;
import frc.robot.Constants;
import frc.robot.subsystems.LowerArmSubsystem;
import frc.robot.subsystems.UpperArmSubsystem;

public class RaiseArmCommand extends CommandBase {
  LowerArmSubsystem lowerArmSubsystem;
  UpperArmSubsystem upperArmSubsystem;
  
  /** Creates a new RaiseArmCommand. */
  public RaiseArmCommand(LowerArmSubsystem lowerArmSubsystem, UpperArmSubsystem upperArmSubsystem) {
    this.lowerArmSubsystem = lowerArmSubsystem;
    this.upperArmSubsystem = upperArmSubsystem;
  }

  // Called when the command is initially scheduled.
  @Override
  public void initialize() {
    lowerArmSubsystem.raise(); // Raises lower arm
    upperArmSubsystem.setPosition(Constants.upperArmRaisedPosition); // Tells upper arm to move to raised position, but takes time
  }

  // Called every time the scheduler runs while the command is scheduled.
  @Override
  public void execute() {
    // Adjusts lower arm to account for movement in upper arm
    lowerArmSubsystem.accountForUpperArmMovement(upperArmSubsystem);
  }

  // Called once the command ends or is interrupted.
  @Override
  public void end(boolean interrupted) {
    lowerArmSubsystem.lock(); // Lock the lower arm in place
  }

  // Returns true when the arm reaches position
  @Override
  public boolean isFinished() {
    return upperArmSubsystem.reachedPosition();
  }
}

Then, to call it in RobotContainer.java:

// Above constructor:
CommandXboxController controller = new CommandXboxController(0);

Trigger raiseArmTrigger = controller.a();

LowerArmSubsystem lowerArmSubsystem = new LowerArmSubsystem(TalonFX motor, DoubleSolenoid lockingSolenoid);
UpperArmSubsystem upperArmSubsystem = new UpperArmSubsystem(TalonFX motor, Encoder encoder);

RaiseArmCommand raiseArmCommand = new RaiseArmCommand(lowerArmSubsystem, upperArmSubsystem);

// In configureBindings:
raiseArmTrigger.whileTrue(raiseArmCommand);

Closing a Claw

For a simple inline instant command to just close the claw:

// In configureBindings
closeClawTrigger.onTrue( new InstantCommand(() -> clawSubsystem.close(), clawSubsystem) );