3. Create a Teleop Command

In this tutorial, we will learn how to create robot commands. Commands are the basic units for implementing robot behavior in WPILib, encapsulating specific actions or behavior sequences. We will reference the structure of the ChassisTeleop command to create a new IntakeTeleop command. Code download link

WPILib's command framework is based on the command pattern design and has the following key characteristics:

• Composability: Commands can be composed into more complex behavior sequences, supporting sequential, parallel, and conditional execution
• Interruptibility: Commands can be safely interrupted at any time, ensuring robot state consistency
• Subsystem Management: Automatically handles subsystem acquisition and release, preventing resource conflicts
• State Encapsulation: Each command encapsulates complete behavioral logic, facilitating testing and reuse

Example

Let's analyze the structure of the ChassisTeleop command to understand the key components of a command:

public class ChassisTeleop extends Command {
  // a. Dependency declarations

  // b. Constructor
  public ChassisTeleop(
      Chassis chassis, DoubleSupplier forwardSupplier, DoubleSupplier rotationSupplier) {
  }

  // c. Initialization method - called when the command starts
  @Override
  public void initialize() {
  }

  // d. Execution method - called periodically
  @Override
  public void execute() {
  }

  // e. End method - called when the command ends
  @Override
  public void end(boolean interrupted) {
  }

  // f. Returns whether the command is finished
  @Override
  public boolean isFinished() {
  }
}

a. Dependency Declarations

public class ChassisTeleop extends Command {
  private final Chassis chassis;
  private final DoubleSupplier forwardSupplier;
  private final DoubleSupplier rotationSupplier;
}

The command injects the required dependencies through the constructor:

• chassis: The subsystem instance to control
• DoubleSupplier: A functional interface that provides input values

b. Constructor

public class ChassisTeleop extends Command {
  public ChassisTeleop(
      Chassis chassis, DoubleSupplier forwardSupplier, DoubleSupplier rotationSupplier) {
    this.chassis = chassis;
    this.forwardSupplier = forwardSupplier;
    this.rotationSupplier = rotationSupplier;
    addRequirements(chassis); // Key: Declare the subsystem required by the command
  }
}

The addRequirements() method informs the command scheduler that this command requires exclusive access to the chassis subsystem. While this command is running, other commands requiring the same subsystem will be blocked.

c. Initialization Method

public class ChassisTeleop extends Command {
  @Override
  public void initialize() {
  }
}

The initialize() method is called once after the command is instantiated and before the execute() method is repeatedly run. In this example, there is no initialization required.

d. Execution Method

public class ChassisTeleop extends Command {
  @Override
  public void execute() {
    // Input processing
    double forward = 5.0 * Math.abs(forwardSupplier.getAsDouble()) * forwardSupplier.getAsDouble();
    double rotation = 3.0 * Math.abs(rotationSupplier.getAsDouble()) * rotationSupplier.getAsDouble();

    // Control logic
    if (forward < 0) {
      double leftSpeed = forward + rotation;
      double rightSpeed = forward - rotation;
      chassis.setWheelsVelocities(leftSpeed, rightSpeed);
    } else {
      double leftSpeed = forward - rotation;
      double rightSpeed = forward + rotation;
      chassis.setWheelsVelocities(leftSpeed, rightSpeed);
    }
  }
}

The execute() method is called during each scheduling cycle (approximately every 20ms):

• Input Processing: Applies a nonlinear transformation to the raw input for better control experience
• Control Logic: Adjusts differential calculations based on the forward direction to ensure intuitive control response

e. End Method

public class ChassisTeleop extends Command {
  @Override
  public void end(boolean interrupted) {
    chassis.setWheelsVelocities(0, 0);
  }
}

The end() method is called when the command ends:

• The interrupted parameter indicates whether the command ended normally or was interrupted
• Ensures the subsystem returns to a safe state

f. Completion Condition

public class ChassisTeleop extends Command {
  @Override
  public boolean isFinished() {
    return false; // Runs continuously
  }
}

For teleoperation commands, it is common to return false to keep the command running continuously until the operator stops it or a higher-priority command interrupts it.

Your Turn

Now you need to reference the structure of ChassisTeleop to complete the design of the IntakeTeleop command. This command should control the intake roller movement and arm angle based on four boolean inputs.

public class IntakeTeleop extends Command {
  private final Intake intake;
  private final BooleanSupplier rollerInjectSupplier;
  private final BooleanSupplier rollerEjectSupplier;
  private final BooleanSupplier armUpSupplier;
  private final BooleanSupplier armDownSupplier;

  public IntakeTeleop(
      Intake intake,
      BooleanSupplier rollerInjectSupplier,
      BooleanSupplier rollerEjectSupplier,
      BooleanSupplier armUpSupplier,
      BooleanSupplier armDownSupplier) {
    this.intake = intake;
    this.rollerInjectSupplier = rollerInjectSupplier;
    this.rollerEjectSupplier = rollerEjectSupplier;
    this.armUpSupplier = armUpSupplier;
    this.armDownSupplier = armDownSupplier;
    addRequirements(intake);
  }

  @Override
  public void execute() {
    // TODO: implement intake teleop control

    // when both buttons are pressed, do nothing
    // when inject button is pressed, intake
    // when eject button is pressed, outtake

    // when both buttons are pressed, do nothing
    // when arm up button is pressed, raise the arm
    // when arm down button is pressed, lower the arm
  }

  @Override
  public void end(boolean interrupted) {
    // TODO: stop the rollers and hold the arm position
  }

  @Override
  public boolean isFinished() {
    return false;
  }
}

Demonstration

Control the robot's left and right wheels forward using the left and right joysticks respectively. The right trigger controls roller intake, the left trigger controls roller outtake. The left shoulder button raises the intake angle, and the right shoulder button lowers the intake angle.