LESSON 27

Autonomous

One autonomous routine is one class holding one command. The class puts a name on the driver station and owns the mode boundary. The command does the driving, and the one you build here leaves the starting line and stops.

Branchswerve-autonomous30 minutes

You’ll need

A swerve robot you can drive, with a pose you trust, from Swerve Drive Tuning.
run(coroutine -> ...) and coroutine.wait(...), from Coroutines.
Three meters of clear floor and one person on the disable switch.

You have written an @Autonomous class once already: Raise And Shoot, on Coroutines, ran the arm and flywheel with no one holding a button. This one has the same shape and drives the whole robot.

The routine is a timed drive, and it is crude on purpose. A timed step tells you whether the mode list, the scheduler, and the drivetrain agree with each other. It tells you very little about where the robot ended up.

Two layers

The class is the part you cannot test without a driver station. Keep everything else out of it. A command that reads a controller, checks the match clock, or names a mode has taken on the class's job.

The @Autonomous class: The name on the driver station, the Robot handed to its constructor, and the schedule and cancel calls at the mode boundary.
The routine command: Which way to drive, for how long, and how it stops. The same command can run from a button or from another routine unchanged.

A command built on the drivetrain requires the drivetrain, so a second drivetrain command cannot run beside it. Arm and flywheel commands can. That is the same resource rule the scheduler has enforced since Workshop 2.

Build the routine

Everything gets built in the constructor, which runs the moment somebody picks the mode. The routine lives in a field because end() needs a reference to the command it cancels. Building a command sends no output, so the constructor is safe to run while the robot is still disabled. start() runs when the mode is enabled, and end() runs when it stops for any reason, a disable included.

LeaveStartAuto.java: lifecycle and behavior together

1package frc.robot.opmodes;
2 
3import static org.wpilib.units.Units.Seconds;
4 
5import com.ctre.phoenix6.swerve.SwerveRequest;
6import frc.robot.Robot;
7import org.wpilib.command3.Command;
8import org.wpilib.command3.Scheduler;
9import org.wpilib.opmode.Autonomous;
10import org.wpilib.opmode.PeriodicOpMode;
11 
12@Autonomous(name = "Leave Start")
13public class LeaveStartAuto extends PeriodicOpMode {
14  private final Command routine;
15 
16  public LeaveStartAuto(Robot robot) {
17    // Robot-centric: X is the robot's own forward, so the starting heading sets the direction.
18    final var forward = new SwerveRequest.RobotCentric().withVelocityX(1.0); // meters per second
19    final var stopped = new SwerveRequest.RobotCentric(); // every speed is zero
20 
21    routine =
22        robot
23            .drivetrain
24            .run(
25                coroutine -> {
26                  robot.drivetrain.setControl(forward);
27                  coroutine.wait(Seconds.of(1.5));
28                  robot.drivetrain.setControl(stopped);
29                })
30            .whenCanceled(() -> robot.drivetrain.setControl(stopped))
31            .named("Leave Start");
32  }
33 
34  @Override
35  public void start() {
36    Scheduler.getDefault().schedule(routine);
37  }
38 
39  @Override
40  public void end() {
41    Scheduler.getDefault().cancel(routine);
42  }
43}

A wait is the only finish line available here. DriveMechanism reports its pose, but nothing on it answers am I there yet the way robot.arm.isAtTarget() did on Finish Conditions. PathPlanner, the next lesson, replaces the wait with a drawn path that knows where it ends.

Two names go into this file and they do different jobs. The one in the annotation is what the driver station lists, so it is the one a driver reads under pressure. The one in .named(...) is what the command is called in the scheduler and on the dashboard.

The stopped request is the line people leave out. setControl latches a request: the drivetrain keeps applying it until something sends a different one. When a routine ends, nothing does. This OpMode sets no default command, so the wheels carry on at the last speed they were given.

It is sent twice for that reason. The last line of the coroutine covers a routine that runs to the end. A cancel stops the coroutine where it is and skips that line, so whenCanceled sends the same zero.

The robot's field position is never set in this routine. Drivetrain/Pose starts wherever odometry left off. Restart the robot code before a measured run and it reads near zero, which makes the distance easy to read straight off AdvantageScope.

Four test passes

Each pass answers one question, and each one can fail on its own. Run them in order. A routine that fails the second pass has nothing to prove in the third.

Don't

Nobody in front of the robot

An autonomous routine drives with nobody holding a stick. Give one person the robot to watch and one person the driver station, with a thumb near disable. Keep the first three meters clear of anything you care about. Enable last.

On blocks. Deploy, pick Leave Start off the mode list, and enable. All four modules drive forward together for about a second and a half. Then they stop, and they stay stopped while the mode runs.
On the floor, once. Put the robot on its tape mark with clear floor ahead of it and run the same routine. It leaves in the direction its front bumper points, because RobotCentric X is the robot's forward and not the field's. The starting heading sets the direction.
Measured, three times. Tape the floor at the front edge before and after each run, starting from the same mark every time. The taped distance and the last Drivetrain/Pose in AdvantageScope should agree within a few centimeters. The three runs should land inside about ten.
Disabled partway. Hit disable about a second into the drive. The wheels stop at once. Re-select the mode from the list before running again: picking a mode builds the OpMode fresh, and a fresh routine with it.

A second and a half is a small slice of an autonomous period. A robot that sits still for the rest of it has not failed. That is the stop doing its job.

Three failure shapes

A routine fails quietly. Nothing throws, nothing logs a complaint, and the robot does something you did not ask for. Almost all of it looks like one of these three.

Stuck on a wait: Selected, enabled, sitting still. A wait with no time limit holds the routine there forever. Every coroutine.waitUntil(...) in a routine takes a timeout.
A latched request: The wait ends and the robot keeps rolling. Nothing zeroes the drivetrain, so the last request stays applied. Send stopped on every way out.
Heading or voltage: It moves, and not where you aimed it. Robot-centric X follows the starting heading, and a tired battery shortens a timed step by a surprising amount.

A mode missing from the driver station list is a different problem. Take it back to OpModes: a class that is not public, an annotation with no name, or a constructor that does not take Robot.

Read Drivetrain/Pose before guessing. Its value at the end of a run separates a robot that went the wrong way from one that never went anywhere.

Check your work

Run the routine three times from the same tape mark, on the floor, with AdvantageScope connected. You are done when the three runs land on top of each other.

Check

You should see

Leave Start on the mode list, and the drive beginning the moment you enable.
The robot leaving in the direction its front bumper was pointing.
A full stop that stays stopped, with no creep after the wait.
Three end poses in Drivetrain/Pose within about ten centimeters of each other.

Write down the distance the tape measured, the end pose, and the wait that produced them. PathPlanner replaces that wait with a path drawn from the same tape mark. These three numbers are what you will hold the path against.

A second routine is a second file. Copy this one, change the annotation name and the numbers, and it turns up on the list beside the first. Nothing registers it, and nothing in Robot.java chooses between the two.