11. Drivetrain Project: Autonomous#

Why it matters#

The first 15 seconds of every match have no driver input. The robot runs entirely on code. Autonomous points often decide close matches, and a reliable auto is one of the things alliance captains look for when picking partners.

In this lesson you'll build autonomous routines step by step: first a simple timed drive, then driving by measured distance, then multi-step routines, and finally a dashboard menu so the drive team can pick a routine before each match.

Read this#

• Command Compositions
• Lesson 6.1: A dropdown menu with Selectable

Walkthrough#

How autonomous starts and stops#

In the Command Robot template, Robot.java already handles this:

1. When autonomous begins, autonomousInit() calls robotContainer.getAutonomousCommand() and schedules whatever it returns.
2. The scheduler runs that command every loop.
3. When teleop begins, teleopInit() cancels it, in case it's still running.

So your whole job is to make getAutonomousCommand() return the right command. That command is usually built from smaller commands you already have.

Building routines from small commands#

Command compositions chain commands together like sentences:

Compositions let several things happen at once, then wait for all of them before moving on:

Moving the wrist and elevator in parallel, then scoring once both are done. From the WPILib docs source, concurrent-commands.drawio.svg (CC BY 4.0).

The Commands class has matching helpers for longer lists:

Commands.sequence(a, b, c)        // a, then b, then c
Commands.parallel(a, b)           // all at once, until all end
Commands.race(a, b)               // all at once, until any ends
Commands.deadline(a, b, c)        // all at once, until the first one (a) ends
Commands.waitSeconds(1.0)         // pause

Version 1: a timed auto#

The simplest auto drives at a fixed speed for a fixed time. Add to Drivetrain.java:

public Command driveForSeconds(double speed, double seconds) {
    return run(() -> drive.arcadeDrive(speed, 0, false))   // false = don't square the input
        .withTimeout(seconds)
        .finallyDo(() -> drive.stopMotor())
        .withName("Drive " + seconds + "s");
}

.finallyDo(...) runs when the command ends for any reason: finished, timed out, or canceled. Use it to make sure motors always stop.

Then in RobotContainer.java:

public Command getAutonomousCommand() {
    return drivetrain.driveForSeconds(0.5, 2.0);
}

Run it in the Sim GUI: select Autonomous, then enable.

Version 2: drive by distance#

With encoders, the robot can drive until it has actually gone the right distance:

public Command driveDistance(double meters, double speed) {
    return runOnce(this::resetEncoders)
        .andThen(run(() -> drive.arcadeDrive(Math.copySign(speed, meters), 0, false)))
        .until(() -> Math.abs(getDistanceMeters()) >= Math.abs(meters))
        .finallyDo(() -> drive.stopMotor())
        .withName("Drive " + meters + " m");
}

Reading it line by line:

1. Reset the encoders to 0, once.
2. Then drive at speed, in the direction of meters (negative = backward).
3. Stop as soon as the measured distance reaches the target.
4. Always stop the motors at the end.

This is much more consistent than a timer, but it still overshoots a little, because the robot is moving at full speed when it reaches the target and takes time to stop. In Lesson 12 you'll replace the fixed speed with a PID controller that slows down smoothly as it arrives.

Version 3: a multi-step routine#

Real autos combine several actions. Here's "drive out, score, drive back," using the intake from Lesson 7:

public Command scoreAndBackUp() {
    return Commands.sequence(
        drivetrain.driveDistance(1.5, 0.5).withTimeout(4.0),   // drive out
        intake.outtake().withTimeout(0.5),                     // score
        Commands.waitSeconds(0.25),                            // let the piece clear
        drivetrain.driveDistance(-1.0, 0.5).withTimeout(4.0)   // back up
    ).withName("Score and Back Up");
}

And one that runs mechanisms at the same time, picking up a game piece while driving:

public Command driveAndIntake() {
    return Commands.deadline(
        drivetrain.driveDistance(2.0, 0.4).withTimeout(5.0),   // the deadline
        intake.runIntake()                                     // runs until driving ends
    ).withName("Drive and Intake");
}

deadline ends when the drive finishes, and cancels the intake at the same moment.

Let the drive team choose: Selectable#

Different match strategies need different autos. A Selectable (Lesson 6.1) puts a dropdown menu in Elastic:

private final Selectable<Command> autoChooser = new Selectable<>();

public RobotContainer() {
    // ...subsystems and default commands...

    autoChooser.addDefault("Do nothing", Commands.none());
    autoChooser.add("Drive forward 2 m", drivetrain.driveDistance(2.0, 0.5));
    autoChooser.add("Score and back up", scoreAndBackUp());
    autoChooser.add("Drive and intake", driveAndIntake());
    Tunables.publish("Auto", autoChooser);   // shows up as /Tunables/Auto

    configureBindings();
}

public Command getAutonomousCommand() {
    return autoChooser.getSelected();
}

The drive team picks a routine in Elastic (Tunables → Auto, shown as a ComboBox Chooser) before the match. Make "Do nothing" the default, so a robot whose drive team forgot to choose sits still instead of running a routine meant for a different starting spot.

Testing autos#

1. In simulation first. Watch the dashboard distance and command names. Does each step start and end when you expect?
2. On blocks. Wheels off the ground, check every step runs in order and the motors stop at the end.
3. On the floor, in open space, with a person ready to disable. Mark the starting position with tape so every test starts the same way.
4. Test with a partly drained battery too. That's what you'll have in some matches.

Try it#

• Add driveForSeconds and run it in simulation (select Autonomous, then enable)
• Add driveDistance and confirm on the dashboard that it stops near the target distance
• Build scoreAndBackUp() and watch each step on the scheduler dashboard
• Add a Selectable with at least three options, with "Do nothing" as the default
• Build a routine that uses deadline to run the intake while driving
• Challenge: write turnDegrees(double degrees) using the gyro from Lesson 8, and use it to drive a square

Check yourself#