Command Group programming

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;

  }

}