5. Compose an Autonomous Program

In this tutorial, we will learn how to write an autonomous program. Autonomous programs are a key component in robotics competitions, allowing robots to execute predefined sequences of tasks at the start of a match without operator intervention. Code download link

Framework

WPILib provides a powerful command-based autonomous framework, primarily built on the following building blocks:

1. Instant Command

// Perform an instant action
new InstantCommand(() -> intake.setRollerSpeed(0.5), intake)

2. Wait Command

// Wait for a specified duration
new WaitCommand(2.0) // Wait for 2 seconds
new WaitUntilCommand(() -> intake.hasObject()) // Wait for intake has object

3. Functional Command

// Customize initialization, execution, and ending logic
new FunctionalCommand(
  () -> {},                        // initialize
  () -> chassis.drive(0.5, 0),     // execute
  (interrupted) -> chassis.stop(), // end
  () -> chassis.getDistance() > 5, // isFinished
  chassis                          // requirements
)

4. Conditional Command

// Choose between different commands based on a condition
new ConditionalCommand(
  new AutoScore(),         // Executes if condition is true
  new AutoDrive(),         // Executes if condition is false
  () -> vision.hasTarget() // Condition check
)

5. Command Groups

Sequential Command Group

new SequentialCommandGroup(
  new DriveToLoadingStation(),
  new CollectGamePiece(),
  new DriveToScoringArea(),
  new ScoreGamePiece()
)

Parallel Command Group

new ParallelCommandGroup(
  new RaiseArm(),      // Executes simultaneously
  new DriveForward()   // Executes simultaneously
)

Parallel Deadline Group

// Ends the group when the main command finishes
new ParallelDeadlineGroup(
  new DriveForTime(3.0), // Main command
  new CollectGamePiece() // Accompanying command
)

Example

Let’s analyze a complete autonomous program example that includes the full process of picking and scoring game pieces:

public class RobotContainer {
  private void configureAuto() {
  // ...
  autoCmdSelector.addCommand(
    "Tutorial",
    new SequentialCommandGroup(
      odometry.resetPoseCommand(() -> Field.LEFT_START_POSE),
      new FollowTrajectory(chassis, () -> trajectorySet.score1.get()),
      new ScoreCoral(intake),
      new FollowTrajectory(chassis, () -> trajectorySet.collect1.get())
        .alongWith(new PickCoral(intake))
    ));
  }
}

Your Turn

Now you need to enhance the autonomous program by adding more movement and operation sequences:

public class RobotContainer {
  private void configureAuto() {
  // ...
  autoCmdSelector.addCommand(
    "Tutorial",
    new SequentialCommandGroup(
      odometry.resetPoseCommand(() -> Field.LEFT_START_POSE),
      new FollowTrajectory(chassis, () -> trajectorySet.score1.get()),
      new ScoreCoral(intake),
      new FollowTrajectory(chassis, () -> trajectorySet.collect1.get())
        .alongWith(new PickCoral(intake))
      // TODO: follow `score2` and score
      // TODO: follow `collect2` and collect
      // TODO: follow `score3` and score
    ));
  }
}

Demonstration

The completed autonomous program will enable the robot to:

1. Move from the starting position to the first scoring point and score the game piece.
2. Move to the collection point to collect a new game piece.
3. Repeat the scoring and collection process multiple times.