5. 编写一个自动程序

在本教程中，我们将学习如何编写自动程序。自动程序是机器人竞赛中的关键组成部分，它让机器人在比赛开始时无需操作员干预就能执行预定义的任务序列。代码下载链接

程序框架

WPILib 提供了强大的命令式自动程序框架，主要基于以下几种构建块：

1. 即时命令 (InstantCommand)

// 执行一次性的简单操作
new InstantCommand(() -> intake.setRollerSpeed(0.5), intake)

2. 等待命令 (WaitCommand)

// 等待指定时间
new WaitCommand(2.0) // 等待2秒
new WaitUntilCommand(() -> intake.hasObject()) // 等待得到物体

3. 功能命令 (FunctionalCommand)

// 自定义初始化、执行、结束逻辑
new FunctionalCommand(
  () -> {},                        // initialize
  () -> chassis.drive(0.5, 0),     // execute
  (interrupted) -> chassis.stop(), // end
  () -> chassis.getDistance() > 5, // isFinished
  chassis                          // requirements
)

4. 条件命令 (ConditionalCommand)

// 根据条件选择执行不同的命令
new ConditionalCommand(
    new AutoScore(),         // 条件为true时执行
    new AutoDrive(),         // 条件为false时执行
    () -> vision.hasTarget() // 条件判断
)

5. 命令组

顺序命令组 (SequentialCommandGroup)

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

并行命令组 (ParallelCommandGroup)

new ParallelCommandGroup(
    new RaiseArm(),      // 同时执行
    new DriveForward()   // 同时执行
)

并行限期命令组 (ParallelDeadlineGroup)

// 主命令完成后整个组结束
new ParallelDeadlineGroup(
    new DriveForTime(3.0), // 主命令
    new CollectGamePiece() // 伴随命令
)

示例程序

让我们分析一个完整的自动程序示例，它包含拾取和投放珊瑚的完整流程：

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))
        ));
  }
}

你的回合

现在你需要完善自动程序，添加更多的移动和操作序列：

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
        ));
  }
}

效果演示

完成后的自动程序将使机器人能够：

1. 从起始位置移动到第一个投放点并投放珊瑚
2. 移动到收集点收集新的珊瑚
3. 重复执行投放和收集流程多次