The last example showed two simple commands and how to connect them with the user interface, but the commands were simple: they set a servo position and immediately finished. In practice, robot commands often need to run continuously until something happens; for example, a command to raise an arm might start a motor moving and continue until some time has passed or an encoder indicates that the arm has moved a certain distance or a limit switch is pressed. WPILib commands make this easy. Even better, you can string several such commands together in a CommandGroup to effect complex behaviors.

Commands can be grouped together to run in sequence for example, when the X button is pressed, the following code will run the servo open command, allow time for it to finish, and then run the servo close command. The OpenCommand adds a Timer to allow the servo motor time to complete moving to the open position before the CloseCommand is run :

Constants.java

package frc.robot;

public final class Constants {
    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   
    public static int OPEN_THEN_CLOSE_BUTTON = 3; // X button opens then closes
}

ServoSubSystem.java:

package frc.robot.subsystems;

import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;
import edu.wpi.first.wpilibj.Servo;

public class ServoSubsystem extends SubsystemBase {
  private Servo servo;
  private static ServoSubsystem instance = null;

  private ServoSubsystem() {
      servo = new Servo(Constants.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);
  }
}

OpenCommand.java

package frc.robot.commands;

import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.subsystems.ServoSubsystem;
import edu.wpi.first.wpilibj.Timer;

public class OpenCommand extends CommandBase {
  private Timer delayTimer;

  public OpenCommand(SubsystemBase servoSubsystem) {
      delayTimer = new Timer();
      addRequirements(servoSubsystem);
  }

  @Override
  public void initialize() {
    ServoSubsystem.getInstance().setAngle(30); // set servo to open position
    delayTimer.start();
  }

  @Override
  public boolean isFinished() {
    return delayTimer.hasPeriodPassed(0.5); // allow 500ms for command to finish
  }
}

CloseCommand.java

package frc.robot.commands;

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

public class CloseCommand extends CommandBase {

    public CloseCommand(SubsystemBase servoSubsystem) {
        addRequirements(servoSubsystem);
    }

    @Override
    public boolean isFinished() {
        ServoSubsystem.getInstance().setAngle(120); // set servo to closed position
        return true;                 // and then finish
    }
}

OpenThenCloseCommand.java:

package frc.robot.commands;

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

public class OpenThenCloseCommand extends SequentialCommandGroup {

    public OpenThenCloseCommand(SubsystemBase servoSubsystem) {
        this.addCommands(new OpenCommand(servoSubsystem), new CloseCommand(servoSubsystem));
    }
}

RobotContainer.java:

package frc.robot;

import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj2.command.button.Button;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.ServoSubsystem;
import frc.robot.commands.OpenCommand;
import frc.robot.commands.CloseCommand;
import frc.robot.commands.OpenThenCloseCommand;

public class RobotContainer {
  private final ServoSubsystem m_servo = ServoSubsystem.getInstance();
  private final XboxController xbox;
  private final Button openButton, closeButton, openThenCloseButton;

  public RobotContainer() {
    xbox = new XboxController(0);
    openButton = new JoystickButton(xbox, Constants.OPEN_BUTTON);
    closeButton = new JoystickButton(xbox, Constants.CLOSE_BUTTON);   
    openThenCloseButton = new JoystickButton(xbox, Constants.OPEN_THEN_CLOSE_BUTTON);

    // Configure the button bindings
    configureButtonBindings();
  }

  private void configureButtonBindings() {
    openButton.whenPressed(new OpenCommand(m_servo));
    closeButton.whenPressed(new CloseCommand(m_servo));
    openThenCloseButton.whenPressed(new OpenThenCloseCommand(m_servo));
  }

  public Command getAutonomousCommand() {
    return null;
  }
}