Structuring Your Project¶

While WPILib and the commands v3 framework are flexible and don’t force code to be written in a specific way, we recommend a standard structure to make code easier to understand (both for programmers and CSA volunteers).

Write a single Robot class that owns the mechanisms and driver controls.
Write separate classes that implement Mechanism to represent each mechanism on the robot.
Use OpMode classes to define OpMode-specific behaviors.
Any commands that require a single mechanism should be written as Command methods in that mechanism’s class.
Any commands that require multiple mechanisms should be written as Command methods in a related helper class.

A Single Robot Class¶

WPILib needs a single entry point to start your program. Use a Robot class that extends from OpModeRobot. Any behavior that should always be active - regardless of OpMode - should be defined here, such as default commands or trigger bindings.

Mechanisms should be declared as public final fields in the robot class and initialized in the field declaration or in the constructor. The former makes the code a little more concise, while the latter allows for flexibility if different mechanism implementations exist.

@Logged
public class Robot extends OpModeRobot {
  public final SwerveDrive swerveDrive = new SwerveDrive();
  public final Shooter shooter = new Shooter();
  public final Intake intake = new Intake();
  public final PoseEstimator poseEstimator = new PoseEstimator();
  public final CommandGamepad controller = new CommandGamepad(1);

  public final Trigger inNeutralZone = new Trigger(() -> poseEstimator.inZone(NEUTRAL_ZONE));

  // Store the scheduler in a field for Epilogue to pick up and log for us
  private final Scheduler scheduler = Scheduler.getDefault();

  /** Initializes the robot class and sets safe default commands for all its mechanisms. */
  public Robot() {
    swerveDrive.setDefaultCommand(swerveDrive.idle());
    shooter.setDefaultCommand(shooter.idle());
    intake.setDefaultCommand(intake.idle());
  }

  @Override
  public void robotPeriodic() {
    // 1. Update odometry so commands will be working with up-to-date information.
    poseEstimator.odometryUpdate(swerveDrive.getGyroHeading(), swerveDrive.getModulePositions());

    // 2. Run the scheduler to poll triggers and execute our commands.
    scheduler.run();

    // 3. Update telemetry.
    Epilogue.update(this);
  }

  /**
   * A command that uses all the mechanisms on the robot to aim and shoot at a specific target.
   *
   * @param targetPose The target position to shoot at.
   * @return A command to shoot at a target
   */
  public Command shootAt(Pose2d targetPose) {
    return Command.noRequirements(
            coroutine -> {
              coroutine.await(swerveDrive.aimAt(poseEstimator::getEstimatedPose, targetPose));

              coroutine.awaitAll(
                  intake.agitate(),
                  shooter.shootAtHub(
                      () -> {
                        return Meters.of(
                            targetPose
                                .minus(poseEstimator.getEstimatedPose())
                                .getTranslation()
                                .getNorm());
                      }));
            })
        .named("Robot.ShootAt[" + targetPose + "]");
  }
}

Mechanism Classes¶

Each physical mechanism on a robot should have a corresponding class in the codebase. These are typically in the first.robot.mechanisms package.

Mechanisms are often a combination of multiple actuators, such as a slapdown intake with one actuator or set of actuators to run intake rollers and a separate actuator that extends and retracts the intake. It’s usually simpler for each independent actuator to have its own Mechanism class; if they’re only used in the context of a larger mechanism, they can still be separate classes, but only used by a single encompassing mechanism.

Mechanism-level Commands¶

Commands that only interact with a single mechanism should be defined as methods in that mechanism’s class using the run or runRepeatedly builder methods provided by the Mechanism interface. These builder methods make the created command automatically require the mechanism so you can’t forget to set the requirement.

Warning

Command methods are designed to create a Command object that will be run at a later point in the program. Only the code in the lambda function passed to run or runRepeatedly will execute when the command is running.

In the example of a slapdown intake, there could be three classes:

IntakeRoller, for controlling just the rollers
IntakeWrist, for controlling the deployment of the intake
Intake, which combines both the rollers and the wrist

`IntakeRoller.java`¶

@Logged
public class IntakeRoller implements Mechanism {
  private final ExampleSmartMotorController motor =
      new ExampleSmartMotorController(IntakeConstants.ROLLER_MOTOR_ID);

  /**
   * Stops the roller motor. This command keeps the roller off until interrupted by another command.
   *
   * @return Command to stop the roller
   */
  public Command stop() {
    return runRepeatedly(() -> motor.setThrottle(0.0)).named("Intake.Roller.Stop");
  }

  @Override
  public Command idle() {
    return stop();
  }

  /**
   * Starts the roller motor in intake mode. This command keeps the roller running until interrupted
   * by another command.
   *
   * @return Command to start the roller in intake mode
   */
  public Command intake() {
    return run(coroutine -> {
          motor.setSetpoint(
              ExampleSmartMotorController.PIDMode.VELOCITY,
              IntakeConstants.ROLLER_INTAKE_SPEED.in(RadiansPerSecond));

          coroutine.park();
        })
        .named("Intake.Roller.Intake");
  }

  /**
   * Starts the roller motor in expel mode. This command keeps the roller running until interrupted
   * by another command.
   *
   * @return Command to start the roller in expel mode
   */
  public Command expel() {
    return run(coroutine -> {
          motor.setSetpoint(
              ExampleSmartMotorController.PIDMode.VELOCITY,
              IntakeConstants.ROLLER_EXPULSION_SPEED.in(RadiansPerSecond));

          coroutine.park();
        })
        .named("Intake.Roller.Expel");
  }
}

`IntakeWrist.java`¶

@Logged
public class IntakeWrist implements Mechanism {
  private final ExampleSmartMotorController motor =
      new ExampleSmartMotorController(IntakeConstants.WRIST_MOTOR_ID);

  public Command stop() {
    return runRepeatedly(() -> motor.setThrottle(0.0)).named("Intake.Wrist.Stop");
  }

  @Override
  public Command idle() {
    return stop();
  }

  /**
   * Moves the wrist to a specific angle. The command will exit when the target angle is reached.
   *
   * @param target The target angle to move to
   * @return A command that moves the wrist to the target angle
   */
  public Command moveToAngle(Angle target) {
    double targetRads = target.in(Radians);
    double tolerance = Radians.convertFrom(0.5, Degrees);

    return run(coroutine -> {
          motor.setSetpoint(ExampleSmartMotorController.PIDMode.POSITION, targetRads);

          coroutine.waitUntil(() -> Math.abs(motor.getEncoderDistance() - targetRads) <= tolerance);
        })
        .named("Intake.Wrist.MoveToAngle[" + target.toLongString() + "]");
  }

  /**
   * Moves the wrist to a specific angle. The command will hold the wrist at the target angle until
   * interrupted.
   *
   * @param target The target angle to hold
   * @return A command that holds the wrist at the target angle
   */
  public Command holdAngle(Angle target) {
    double targetRads = target.in(Radians);

    return run(coroutine -> {
          motor.setSetpoint(ExampleSmartMotorController.PIDMode.POSITION, targetRads);
          coroutine.park();
        })
        .named("Intake.Wrist.HoldAngle[" + target.toLongString() + "]");
  }

  /**
   * Moves the wrist to the stowed position and holds it there until interrupted.
   *
   * @return A command that stows the wrist
   */
  public Command stow() {
    return holdAngle(IntakeConstants.WRIST_STOW_ANGLE);
  }

  /**
   * Moves the wrist to the down position and holds it there until interrupted.
   *
   * @return A command that moves the wrist to the down position
   */
  public Command down() {
    return holdAngle(IntakeConstants.WRIST_DOWN_ANGLE);
  }
}

`Intake.java`¶

@Logged
public class Intake implements Mechanism {
  private final IntakeWrist wrist = new IntakeWrist();
  private final IntakeRoller roller = new IntakeRoller();

  /**
   * Intakes fuel off the ground. This command will run until canceled or interrupted by another
   * command.
   *
   * @return An intake command
   */
  public Command intake() {
    return run(coroutine -> {
          coroutine.awaitAll(wrist.down(), roller.intake());
        })
        .named("Intake.Intake");
  }

  /**
   * Stops the intake mechanism. This stops the roller and depowers the wrist, which may sag under
   * gravity.
   *
   * @return A command that stops the intake mechanism
   */
  public Command stop() {
    return run(coroutine -> {
          coroutine.awaitAll(wrist.stop(), roller.stop());
        })
        .withPriority(Command.LOWEST_PRIORITY)
        .named("Intake.Idle");
  }

  @Override
  public Command idle() {
    return stop();
  }

  /**
   * Stows the intake mechanism. This raises the wrist and stops the roller.
   *
   * @return A command that stows the intake mechanism
   */
  public Command stow() {
    return run(coroutine -> {
          coroutine.awaitAll(wrist.stow(), roller.stop());
        })
        .named("Intake.Stow");
  }

  /**
   * Uses the intake mechanism to agitate fuel inside the hopper. The roller will spin to encourage
   * fuel to move around and help unblock jams.
   *
   * @return A command that agitates the intake mechanism
   */
  public Command agitate() {
    return run(coroutine -> {
          coroutine.fork(roller.intake());

          while (true) {
            // await() will yield internally until the wrist has moved to the desired position,
            // so we don't need to explicitly yield here
            coroutine.await(wrist.moveToAngle(IntakeConstants.WRIST_AGITATE_UP));
            coroutine.await(wrist.moveToAngle(IntakeConstants.WRIST_AGITATE_DOWN));
          }
        })
        .named("Intake.Agitate");
  }

  /**
   * Uses the intake mechanism to expel fuel from the hopper.
   *
   * @return A command that expels fuel from the intake mechanism
   */
  public Command outtake() {
    return run(coroutine -> {
          coroutine.awaitAll(wrist.down(), roller.expel());
        })
        .named("Intake.Outtake");
  }
}

Multi-Mechanism Commands¶

Commands that control multiple mechanisms should coordinate smaller per-mechanism commands. From the intake example above, take note that the main Intake class has no hardware of its own; all hardware control is done in the separate roller and wrist mechanisms, while Intake is only responsible for coordinating the two.

Not every multi-mechanism control scheme needs to use a encapsulating mechanism to coordinate control. The intake example uses that setup because the wrist and roller are part of the intake, but control of disparate mechanisms like an intake and an arm can use a no-requirement command that forks and coordinates mechanism-specific commands.

Complex multi-mechanism control can also be done using state machines.

OpMode Classes¶

OpMode classes let you group mode-specific logic together in one place without cluttering the main robot class. Because the command scheduler automatically scopes everything you do in the OpMode class to that OpMode, you don’t need to worry about logic specific to that OpMode leaking out and still running when modes change. Commands that are only used in a specific OpMode can be defined in that OpMode class, too.

OpMode constructors are generally all that’s needed. WPILib will automatically call them when that mode is selected on the driver station, passing in the main Robot object if the constructor accepts it.

@Autonomous
public class SweepAuto implements OpMode {
  private final Trigger enabled = new Trigger(RobotState::isEnabled);

  /**
   * Creates a new autonomous mode that sweeps the left trench and scores on the hub. This
   * constructor is called automatically by the OpModeRobot framework when the program starts up.
   *
   * @param robot The robot instance to control.
   */
  public SweepAuto(Robot robot) {
    // Start the intake stowed
    robot.intake.setDefaultCommand(robot.intake.stow());

    // Extend the intake once we cross into the neutral zone and leave it extended for the entire
    // opmode
    robot.inNeutralZone.onTrue(robot.intake.intake());

    // Once the robot is enabled, start following a sweep path through the left trench,
    // into the neutral zone, then back over the bump.
    // When we return to the alliance zone, aim at the hub and start shooting.
    enabled.onTrue(sweepAndScore(robot));
  }

  /**
   * Creates a command that drives the robot in a path that sweeps through the neutral zone to
   * gather fuel, returns to the alliance zone, and then shoots at the hub until the end of the
   * autonomous period.
   *
   * @param robot The robot instance to control
   * @return A sweep-and-score command
   */
  private Command sweepAndScore(Robot robot) {
    return Command.noRequirements(
            coroutine -> {
              coroutine.await(robot.swerveDrive.followPath("nz-sweep-left-trench"));

              FieldConstants.targetHub()
                  .ifPresent(
                      hub -> {
                        coroutine.await(robot.shootAt(hub));
                      });
            })
        .named("Sweep and Score");
  }
}

package first.robot.mechanisms;

import static org.wpilib.units.Units.*;

import module wpilib;
import module wpilib.command3;

public class Arm implements Mechanism {
  // ... hardware definitions, constructors, PID controllers and so on ...

  // Good example:
  public Command stop() {
    return runRepeatedly(() -> motor.stop()).named("Stop Arm");
  }

  public Command incorrectStop() {
    // Incorrect: This will print to console when the command is created, not when it actually runs
    System.out.println("Stopping the arm motor! (but not really)");

    return runRepeatedly(() -> motor.stop()).named("Incorrect Stop Arm");
  }
}

Highly complicated commands with a lot of logic can harm readability of a mechanism class, and can optionally be implemented as standalone class-based commands in the same package as the mechanism. The mechanism class should still have a method to create and return these commands.

As a rule of thumb, commands with more than 15-20 lines of code are good candidates for being moved to class-based commands. Bits of logic can be more easily split into smaller helper methods, but you will need to implement all of the required Command methods yourself.

package first.robot.mechanisms;

import module wpilib.command3;
import first.robot.commands.drivetrain.VeryComplicatedCommand;

public class Drivetrain implements Mechanism {
  // ... hardware definitions, constructors, PID controllers and so on ...

  public Command veryComplicatedCommand() {
    return new VeryComplicatedCommand(this);
  }
}

package first.robot.commands.drivetrain;

import module java.base;
import module wpilib.command3;

public class VeryComplicatedCommand implements Command {
  private final Drivetrain drivetrain;
  private final Set<Mechanism> requirements;

  public FollowPathPartsCommand(Drivetrain drivetrain) {
    this.drivetrain = drivetrain;
    this.requirements = Set.of(drivetrain);
  }

  @Override
  public void run(Coroutine coroutine) {
    // ... lots of complicated logic ...
  }

  @Override
  public Set<Mechanism> requirements() {
    return requirements;
  }

  @Override
  public String name() {
    return "VeryComplicatedCommand";
  }
}

Structuring Your Project¶

A Single Robot Class¶

Mechanism Classes¶

Mechanism-level Commands¶

IntakeRoller.java¶

IntakeWrist.java¶

Intake.java¶

Multi-Mechanism Commands¶

OpMode Classes¶

`IntakeRoller.java`¶

`IntakeWrist.java`¶

`Intake.java`¶