Add new Java Command framework (#1682)

The old command framework is still available, but will be deprecated.

Due to name conflicts, the new framework is in the wpilibj2 package.
Eventually (after the old command framework is removed in a future year)
it will be moved into the main wpilibj package.

wpilibj/build.gradle

@@ -55,6 +55,10 @@ compileJava {
     dependsOn generateJavaVersion
 }
 
+repositories {
+    jcenter()
+}
+
 dependencies {
     compile project(':hal')
     compile project(':wpiutil')
@@ -62,6 +66,7 @@ dependencies {
     compile project(':cscore')
     compile project(':cameraserver')
     testCompile 'com.google.guava:guava:19.0'
+    testCompile 'org.mockito:mockito-core:2.27.0'
     devCompile project(':hal')
     devCompile project(':wpiutil')
     devCompile project(':ntcore')

wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/Command.java

@@ -0,0 +1,304 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.Set;
+import java.util.function.BooleanSupplier;
+
+/**
+ * A state machine representing a complete action to be performed by the robot.  Commands are
+ * run by the {@link CommandScheduler}, and can be composed into CommandGroups to allow users to
+ * build complicated multi-step actions without the need to roll the state machine logic themselves.
+ *
+ * <p>Commands are run synchronously from the main robot loop; no multithreading is used, unless
+ * specified explicitly from the command implementation.
+ */
+@SuppressWarnings("PMD.TooManyMethods")
+public interface Command {
+
+  /**
+   * The initial subroutine of a command.  Called once when the command is initially scheduled.
+   */
+  default void initialize() {
+  }
+
+  /**
+   * The main body of a command.  Called repeatedly while the command is scheduled.
+   */
+  default void execute() {
+  }
+
+  /**
+   * The action to take when the command ends.  Called when either the command finishes normally,
+   * or when it interrupted/canceled.
+   *
+   * @param interrupted whether the command was interrupted/canceled
+   */
+  default void end(boolean interrupted) {
+  }
+
+  /**
+   * Whether the command has finished.  Once a command finishes, the scheduler will call its
+   * end() method and un-schedule it.
+   *
+   * @return whether the command has finished.
+   */
+  default boolean isFinished() {
+    return false;
+  }
+
+  /**
+   * Specifies the set of subsystems used by this command.  Two commands cannot use the same
+   * subsystem at the same time.  If the command is scheduled as interruptible and another
+   * command is scheduled that shares a requirement, the command will be interrupted.  Else,
+   * the command will not be scheduled.  If no subsystems are required, return an empty set.
+   *
+   * <p>Note: it is recommended that user implementations contain the requirements as a field,
+   * and return that field here, rather than allocating a new set every time this is called.
+   *
+   * @return the set of subsystems that are required
+   */
+  Set<Subsystem> getRequirements();
+
+  /**
+   * Decorates this command with a timeout.  If the specified timeout is exceeded before the command
+   * finishes normally, the command will be interrupted and un-scheduled.  Note that the
+   * timeout only applies to the command returned by this method; the calling command is
+   * not itself changed.
+   *
+   * <p>Note: This decorator works by composing this command within a CommandGroup.  The command
+   * cannot be used independently after being decorated, or be re-decorated with a different
+   * decorator, unless it is manually cleared from the list of grouped commands with
+   * {@link CommandGroupBase#clearGroupedCommand(Command)}.  The decorated command can, however, be
+   * further decorated without issue.
+   *
+   * @param seconds the timeout duration
+   * @return the command with the timeout added
+   */
+  default Command withTimeout(double seconds) {
+    return new ParallelRaceGroup(this, new WaitCommand(seconds));
+  }
+
+  /**
+   * Decorates this command with an interrupt condition.  If the specified condition becomes true
+   * before the command finishes normally, the command will be interrupted and un-scheduled.
+   * Note that this only applies to the command returned by this method; the calling command
+   * is not itself changed.
+   *
+   * <p>Note: This decorator works by composing this command within a CommandGroup.  The command
+   * cannot be used independently after being decorated, or be re-decorated with a different
+   * decorator, unless it is manually cleared from the list of grouped commands with
+   * {@link CommandGroupBase#clearGroupedCommand(Command)}.  The decorated command can, however, be
+   * further decorated without issue.
+   *
+   * @param condition the interrupt condition
+   * @return the command with the interrupt condition added
+   */
+  default Command interruptOn(BooleanSupplier condition) {
+    return new ParallelRaceGroup(this, new WaitUntilCommand(condition));
+  }
+
+  /**
+   * Decorates this command with a runnable to run after the command finishes.
+   *
+   * <p>Note: This decorator works by composing this command within a CommandGroup.  The command
+   * cannot be used independently after being decorated, or be re-decorated with a different
+   * decorator, unless it is manually cleared from the list of grouped commands with
+   * {@link CommandGroupBase#clearGroupedCommand(Command)}.  The decorated command can, however, be
+   * further decorated without issue.
+   *
+   * @param toRun the Runnable to run
+   * @return the decorated command
+   */
+  default Command whenFinished(Runnable toRun) {
+    return new SequentialCommandGroup(this, new InstantCommand(toRun));
+  }
+
+  /**
+   * Decorates this command with a runnable to run before this command starts.
+   *
+   * <p>Note: This decorator works by composing this command within a CommandGroup.  The command
+   * cannot be used independently after being decorated, or be re-decorated with a different
+   * decorator, unless it is manually cleared from the list of grouped commands with
+   * {@link CommandGroupBase#clearGroupedCommand(Command)}.  The decorated command can, however, be
+   * further decorated without issue.
+   *
+   * @param toRun the Runnable to run
+   * @return the decorated command
+   */
+  default Command beforeStarting(Runnable toRun) {
+    return new SequentialCommandGroup(new InstantCommand(toRun), this);
+  }
+
+  /**
+   * Decorates this command with a set of commands to run after it in sequence.  Often more
+   * convenient/less-verbose than constructing a new {@link SequentialCommandGroup} explicitly.
+   *
+   * <p>Note: This decorator works by composing this command within a CommandGroup.  The command
+   * cannot be used independently after being decorated, or be re-decorated with a different
+   * decorator, unless it is manually cleared from the list of grouped commands with
+   * {@link CommandGroupBase#clearGroupedCommand(Command)}.  The decorated command can, however, be
+   * further decorated without issue.
+   *
+   * @param next the commands to run next
+   * @return the decorated command
+   */
+  default Command andThen(Command... next) {
+    SequentialCommandGroup group = new SequentialCommandGroup(this);
+    group.addCommands(next);
+    return group;
+  }
+
+  /**
+   * Decorates this command with a set of commands to run parallel to it, ending when the calling
+   * command ends and interrupting all the others.  Often more convenient/less-verbose than
+   * constructing a new {@link ParallelDeadlineGroup} explicitly.
+   *
+   * <p>Note: This decorator works by composing this command within a CommandGroup.  The command
+   * cannot be used independently after being decorated, or be re-decorated with a different
+   * decorator, unless it is manually cleared from the list of grouped commands with
+   * {@link CommandGroupBase#clearGroupedCommand(Command)}.  The decorated command can, however, be
+   * further decorated without issue.
+   *
+   * @param parallel the commands to run in parallel
+   * @return the decorated command
+   */
+  default Command deadlineWith(Command... parallel) {
+    return new ParallelDeadlineGroup(this, parallel);
+  }
+
+  /**
+   * Decorates this command with a set of commands to run parallel to it, ending when the last
+   * command ends.  Often more convenient/less-verbose than constructing a new
+   * {@link ParallelCommandGroup} explicitly.
+   *
+   * <p>Note: This decorator works by composing this command within a CommandGroup.  The command
+   * cannot be used independently after being decorated, or be re-decorated with a different
+   * decorator, unless it is manually cleared from the list of grouped commands with
+   * {@link CommandGroupBase#clearGroupedCommand(Command)}.  The decorated command can, however, be
+   * further decorated without issue.
+   *
+   * @param parallel the commands to run in parallel
+   * @return the decorated command
+   */
+  default Command alongWith(Command... parallel) {
+    ParallelCommandGroup group = new ParallelCommandGroup(this);
+    group.addCommands(parallel);
+    return group;
+  }
+
+  /**
+   * Decorates this command with a set of commands to run parallel to it, ending when the first
+   * command ends.  Often more convenient/less-verbose than constructing a new
+   * {@link ParallelRaceGroup} explicitly.
+   *
+   * <p>Note: This decorator works by composing this command within a CommandGroup.  The command
+   * cannot be used independently after being decorated, or be re-decorated with a different
+   * decorator, unless it is manually cleared from the list of grouped commands with
+   * {@link CommandGroupBase#clearGroupedCommand(Command)}.  The decorated command can, however, be
+   * further decorated without issue.
+   *
+   * @param parallel the commands to run in parallel
+   * @return the decorated command
+   */
+  default Command raceWith(Command... parallel) {
+    ParallelRaceGroup group = new ParallelRaceGroup(this);
+    group.addCommands(parallel);
+    return group;
+  }
+
+  /**
+   * Decorates this command to run perpetually, ignoring its ordinary end conditions.  The decorated
+   * command can still be interrupted or canceled.
+   *
+   * <p>Note: This decorator works by composing this command within a CommandGroup.  The command
+   * cannot be used independently after being decorated, or be re-decorated with a different
+   * decorator, unless it is manually cleared from the list of grouped commands with
+   * {@link CommandGroupBase#clearGroupedCommand(Command)}.  The decorated command can, however, be
+   * further decorated without issue.
+   *
+   * @return the decorated command
+   */
+  default Command perpetually() {
+    return new PerpetualCommand(this);
+  }
+
+  /**
+   * Decorates this command to run "by proxy" by wrapping it in a {@link ProxyScheduleCommand}.
+   * This is useful for "forking off" from command groups when the user does not wish to extend
+   * the command's requirements to the entire command group.
+   *
+   * @return the decorated command
+   */
+  default Command asProxy() {
+    return new ProxyScheduleCommand(this);
+  }
+
+  /**
+   * Schedules this command.
+   *
+   * @param interruptible whether this command can be interrupted by another command that
+   *                      shares one of its requirements
+   */
+  default void schedule(boolean interruptible) {
+    CommandScheduler.getInstance().schedule(interruptible, this);
+  }
+
+  /**
+   * Schedules this command, defaulting to interruptible.
+   */
+  default void schedule() {
+    schedule(true);
+  }
+
+  /**
+   * Cancels this command.  Will call the command's interrupted() method.
+   * Commands will be canceled even if they are not marked as interruptible.
+   */
+  default void cancel() {
+    CommandScheduler.getInstance().cancel(this);
+  }
+
+  /**
+   * Whether or not the command is currently scheduled.  Note that this does not detect whether
+   * the command is being run by a CommandGroup, only whether it is directly being run by
+   * the scheduler.
+   *
+   * @return Whether the command is scheduled.
+   */
+  default boolean isScheduled() {
+    return CommandScheduler.getInstance().isScheduled(this);
+  }
+
+  /**
+   * Whether the command requires a given subsystem.  Named "hasRequirement" rather than "requires"
+   * to avoid confusion with
+   * {@link edu.wpi.first.wpilibj.command.Command#requires(edu.wpi.first.wpilibj.command.Subsystem)}
+   *  - this may be able to be changed in a few years.
+   *
+   * @param requirement the subsystem to inquire about
+   * @return whether the subsystem is required
+   */
+  default boolean hasRequirement(Subsystem requirement) {
+    return getRequirements().contains(requirement);
+  }
+
+  /**
+   * Whether the given command should run when the robot is disabled.  Override to return true
+   * if the command should run when disabled.
+   *
+   * @return whether the command should run when the robot is disabled
+   */
+  default boolean runsWhenDisabled() {
+    return false;
+  }
+
+  default String getName() {
+    return this.getClass().getSimpleName();
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/CommandBase.java

@@ -0,0 +1,83 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.HashSet;
+import java.util.Set;
+
+import edu.wpi.first.wpilibj.Sendable;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+
+/**
+ * A {@link Sendable} base class for {@link Command}s.
+ */
+@SuppressWarnings("PMD.AbstractClassWithoutAbstractMethod")
+public abstract class CommandBase implements Sendable, Command {
+
+  protected String m_name = this.getClass().getSimpleName();
+  protected String m_subsystem = "Ungrouped";
+  protected Set<Subsystem> m_requirements = new HashSet<>();
+
+  /**
+   * Adds the specified requirements to the command.
+   *
+   * @param requirements the requirements to add
+   */
+  public final void addRequirements(Subsystem... requirements) {
+    m_requirements.addAll(Set.of(requirements));
+  }
+
+  @Override
+  public Set<Subsystem> getRequirements() {
+    return m_requirements;
+  }
+
+  @Override
+  public String getName() {
+    return this.getClass().getSimpleName();
+  }
+
+  @Override
+  public void setName(String name) {
+    m_name = name;
+  }
+
+  @Override
+  public String getSubsystem() {
+    return m_subsystem;
+  }
+
+  @Override
+  public void setSubsystem(String subsystem) {
+    m_subsystem = subsystem;
+  }
+
+  /**
+   * Initializes this sendable.  Useful for allowing implementations to easily extend SendableBase.
+   *
+   * @param builder the builder used to construct this sendable
+   */
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    builder.setSmartDashboardType("Command");
+    builder.addStringProperty(".name", this::getName, null);
+    builder.addBooleanProperty("running", this::isScheduled, value -> {
+      if (value) {
+        if (!isScheduled()) {
+          schedule();
+        }
+      } else {
+        if (isScheduled()) {
+          cancel();
+        }
+      }
+    });
+    builder.addBooleanProperty(".isParented",
+        () -> CommandGroupBase.getGroupedCommands().contains(this), null);
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/CommandGroupBase.java

@@ -0,0 +1,124 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.Collection;
+import java.util.Collections;
+import java.util.Set;
+import java.util.WeakHashMap;
+
+/**
+ * A base for CommandGroups.  Statically tracks commands that have been allocated to groups to
+ * ensure those commands are not also used independently, which can result in inconsistent command
+ * state and unpredictable execution.
+ */
+public abstract class CommandGroupBase extends CommandBase implements Command {
+  private static final Set<Command> m_groupedCommands =
+      Collections.newSetFromMap(new WeakHashMap<>());
+
+  static void registerGroupedCommands(Command... commands) {
+    m_groupedCommands.addAll(Set.of(commands));
+  }
+
+  /**
+   * Clears the list of grouped commands, allowing all commands to be freely used again.
+   *
+   * <p>WARNING: Using this haphazardly can result in unexpected/undesirable behavior.  Do not
+   * use this unless you fully understand what you are doing.
+   */
+  public static void clearGroupedCommands() {
+    m_groupedCommands.clear();
+  }
+
+  /**
+   * Removes a single command from the list of grouped commands, allowing it to be freely used
+   * again.
+   *
+   * <p>WARNING: Using this haphazardly can result in unexpected/undesirable behavior.  Do not
+   * use this unless you fully understand what you are doing.
+   *
+   * @param command the command to remove from the list of grouped commands
+   */
+  public static void clearGroupedCommand(Command command) {
+    m_groupedCommands.remove(command);
+  }
+
+  /**
+   * Requires that the specified commands not have been already allocated to a CommandGroup. Throws
+   * an {@link IllegalArgumentException} if commands have been allocated.
+   *
+   * @param commands The commands to check
+   */
+  public static void requireUngrouped(Command... commands) {
+    requireUngrouped(Set.of(commands));
+  }
+
+  /**
+   * Requires that the specified commands not have been already allocated to a CommandGroup. Throws
+   * an {@link IllegalArgumentException} if commands have been allocated.
+   *
+   * @param commands The commands to check
+   */
+  public static void requireUngrouped(Collection<Command> commands) {
+    if (!Collections.disjoint(commands, getGroupedCommands())) {
+      throw new IllegalArgumentException("Commands cannot be added to more than one CommandGroup");
+    }
+  }
+
+  static Set<Command> getGroupedCommands() {
+    return m_groupedCommands;
+  }
+
+  /**
+   * Adds the given commands to the command group.
+   *
+   * @param commands The commands to add.
+   */
+  public abstract void addCommands(Command... commands);
+
+  /**
+   * Factory method for {@link SequentialCommandGroup}, included for brevity/convenience.
+   *
+   * @param commands the commands to include
+   * @return the command group
+   */
+  public static CommandGroupBase sequence(Command... commands) {
+    return new SequentialCommandGroup(commands);
+  }
+
+  /**
+   * Factory method for {@link ParallelCommandGroup}, included for brevity/convenience.
+   *
+   * @param commands the commands to include
+   * @return the command group
+   */
+  public static CommandGroupBase parallel(Command... commands) {
+    return new ParallelCommandGroup(commands);
+  }
+
+  /**
+   * Factory method for {@link ParallelRaceGroup}, included for brevity/convenience.
+   *
+   * @param commands the commands to include
+   * @return the command group
+   */
+  public static CommandGroupBase race(Command... commands) {
+    return new ParallelRaceGroup(commands);
+  }
+
+  /**
+   * Factory method for {@link ParallelDeadlineGroup}, included for brevity/convenience.
+   *
+   * @param deadline the deadline command
+   * @param commands the commands to include
+   * @return the command group
+   */
+  public static CommandGroupBase deadline(Command deadline, Command... commands) {
+    return new ParallelDeadlineGroup(deadline, commands);
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java

@@ -0,0 +1,477 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.ArrayList;
+import java.util.Collection;
+import java.util.Collections;
+import java.util.Iterator;
+import java.util.LinkedHashMap;
+import java.util.LinkedHashSet;
+import java.util.List;
+import java.util.Map;
+import java.util.Set;
+import java.util.function.Consumer;
+
+import edu.wpi.first.hal.FRCNetComm.tInstances;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.wpilibj.RobotState;
+import edu.wpi.first.wpilibj.SendableBase;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+
+/**
+ * The scheduler responsible for running {@link Command}s.  A Command-based robot should call {@link
+ * CommandScheduler#run()} on the singleton instance in its periodic block in order to run commands
+ * synchronously from the main loop.  Subsystems should be registered with the scheduler using
+ * {@link CommandScheduler#registerSubsystem(Subsystem...)} in order for their {@link
+ * Subsystem#periodic()} methods to be called and for their default commands to be scheduled.
+ */
+@SuppressWarnings({"PMD.GodClass", "PMD.TooManyMethods", "PMD.TooManyFields"})
+public final class CommandScheduler extends SendableBase {
+  /**
+   * The Singleton Instance.
+   */
+  private static CommandScheduler instance;
+
+  /**
+   * Returns the Scheduler instance.
+   *
+   * @return the instance
+   */
+  public static synchronized CommandScheduler getInstance() {
+    if (instance == null) {
+      instance = new CommandScheduler();
+    }
+    return instance;
+  }
+
+  //A map from commands to their scheduling state.  Also used as a set of the currently-running
+  //commands.
+  private final Map<Command, CommandState> m_scheduledCommands = new LinkedHashMap<>();
+
+  //A map from required subsystems to their requiring commands.  Also used as a set of the
+  //currently-required subsystems.
+  private final Map<Subsystem, Command> m_requirements = new LinkedHashMap<>();
+
+  //A map from subsystems registered with the scheduler to their default commands.  Also used
+  //as a list of currently-registered subsystems.
+  private final Map<Subsystem, Command> m_subsystems = new LinkedHashMap<>();
+
+  //The set of currently-registered buttons that will be polled every iteration.
+  private final Collection<Runnable> m_buttons = new LinkedHashSet<>();
+
+  private boolean m_disabled;
+
+  //NetworkTable entries for use in Sendable impl
+  private NetworkTableEntry m_namesEntry;
+  private NetworkTableEntry m_idsEntry;
+  private NetworkTableEntry m_cancelEntry;
+
+  //Lists of user-supplied actions to be executed on scheduling events for every command.
+  private final List<Consumer<Command>> m_initActions = new ArrayList<>();
+  private final List<Consumer<Command>> m_executeActions = new ArrayList<>();
+  private final List<Consumer<Command>> m_interruptActions = new ArrayList<>();
+  private final List<Consumer<Command>> m_finishActions = new ArrayList<>();
+
+  CommandScheduler() {
+    HAL.report(tResourceType.kResourceType_Command, tInstances.kCommand_Scheduler);
+    setName("Scheduler");
+  }
+
+  /**
+   * Adds a button binding to the scheduler, which will be polled to schedule commands.
+   *
+   * @param button The button to add
+   */
+  public void addButton(Runnable button) {
+    m_buttons.add(button);
+  }
+
+  /**
+   * Removes all button bindings from the scheduler.
+   */
+  public void clearButtons() {
+    m_buttons.clear();
+  }
+
+  /**
+   * Initializes a given command, adds its requirements to the list, and performs the init actions.
+   *
+   * @param command       The command to initialize
+   * @param interruptible Whether the command is interruptible
+   * @param requirements  The command requirements
+   */
+  private void initCommand(Command command, boolean interruptible, Set<Subsystem> requirements) {
+    command.initialize();
+    CommandState scheduledCommand = new CommandState(interruptible);
+    m_scheduledCommands.put(command, scheduledCommand);
+    for (Consumer<Command> action : m_initActions) {
+      action.accept(command);
+    }
+    for (Subsystem requirement : requirements) {
+      m_requirements.put(requirement, command);
+    }
+  }
+
+  /**
+   * Schedules a command for execution.  Does nothing if the command is already scheduled. If a
+   * command's requirements are not available, it will only be started if all the commands currently
+   * using those requirements have been scheduled as interruptible.  If this is the case, they will
+   * be interrupted and the command will be scheduled.
+   *
+   * @param interruptible whether this command can be interrupted
+   * @param command       the command to schedule
+   */
+  @SuppressWarnings({"PMD.CyclomaticComplexity", "PMD.NPathComplexity"})
+  private void schedule(boolean interruptible, Command command) {
+    if (CommandGroupBase.getGroupedCommands().contains(command)) {
+      throw new IllegalArgumentException(
+          "A command that is part of a command group cannot be independently scheduled");
+    }
+
+    //Do nothing if the scheduler is disabled, the robot is disabled and the command doesn't
+    //run when disabled, or the command is already scheduled.
+    if (m_disabled || (RobotState.isDisabled() && !command.runsWhenDisabled())
+        || m_scheduledCommands.containsKey(command)) {
+      return;
+    }
+
+    Set<Subsystem> requirements = command.getRequirements();
+
+    //Schedule the command if the requirements are not currently in-use.
+    if (Collections.disjoint(m_requirements.keySet(), requirements)) {
+      initCommand(command, interruptible, requirements);
+    } else {
+      //Else check if the requirements that are in use have all have interruptible commands,
+      //and if so, interrupt those commands and schedule the new command.
+      for (Subsystem requirement : requirements) {
+        if (m_requirements.containsKey(requirement)
+            && !m_scheduledCommands.get(m_requirements.get(requirement)).isInterruptible()) {
+          return;
+        }
+      }
+      for (Subsystem requirement : requirements) {
+        if (m_requirements.containsKey(requirement)) {
+          cancel(m_requirements.get(requirement));
+        }
+      }
+      initCommand(command, interruptible, requirements);
+    }
+  }
+
+  /**
+   * Schedules multiple commands for execution.  Does nothing if the command is already scheduled.
+   * If a command's requirements are not available, it will only be started if all the commands
+   * currently using those requirements have been scheduled as interruptible.  If this is the case,
+   * they will be interrupted and the command will be scheduled.
+   *
+   * @param interruptible whether the commands should be interruptible
+   * @param commands      the commands to schedule
+   */
+  public void schedule(boolean interruptible, Command... commands) {
+    for (Command command : commands) {
+      schedule(interruptible, command);
+    }
+  }
+
+  /**
+   * Schedules multiple commands for execution, with interruptible defaulted to true.  Does nothing
+   * if the command is already scheduled.
+   *
+   * @param commands the commands to schedule
+   */
+  public void schedule(Command... commands) {
+    schedule(true, commands);
+  }
+
+  /**
+   * Runs a single iteration of the scheduler.  The execution occurs in the following order:
+   *
+   * <p>Subsystem periodic methods are called.
+   *
+   * <p>Button bindings are polled, and new commands are scheduled from them.
+   *
+   * <p>Currently-scheduled commands are executed.
+   *
+   * <p>End conditions are checked on currently-scheduled commands, and commands that are finished
+   * have their end methods called and are removed.
+   *
+   * <p>Any subsystems not being used as requirements have their default methods started.
+   */
+  @SuppressWarnings({"PMD.CyclomaticComplexity", "PMD.NPathComplexity"})
+  public void run() {
+    if (m_disabled) {
+      return;
+    }
+
+    //Run the periodic method of all registered subsystems.
+    for (Subsystem subsystem : m_subsystems.keySet()) {
+      subsystem.periodic();
+    }
+
+    //Poll buttons for new commands to add.
+    for (Runnable button : m_buttons) {
+      button.run();
+    }
+
+    //Run scheduled commands, remove finished commands.
+    for (Iterator<Command> iterator = m_scheduledCommands.keySet().iterator();
+         iterator.hasNext(); ) {
+      Command command = iterator.next();
+
+      if (!command.runsWhenDisabled() && RobotState.isDisabled()) {
+        command.end(true);
+        for (Consumer<Command> action : m_interruptActions) {
+          action.accept(command);
+        }
+        m_requirements.keySet().removeAll(command.getRequirements());
+        iterator.remove();
+        continue;
+      }
+
+      command.execute();
+      for (Consumer<Command> action : m_executeActions) {
+        action.accept(command);
+      }
+      if (command.isFinished()) {
+        command.end(false);
+        for (Consumer<Command> action : m_finishActions) {
+          action.accept(command);
+        }
+        iterator.remove();
+
+        m_requirements.keySet().removeAll(command.getRequirements());
+      }
+    }
+
+    //Add default commands for un-required registered subsystems.
+    for (Map.Entry<Subsystem, Command> subsystemCommand : m_subsystems.entrySet()) {
+      if (!m_requirements.containsKey(subsystemCommand.getKey())
+          && subsystemCommand.getValue() != null) {
+        schedule(subsystemCommand.getValue());
+      }
+    }
+  }
+
+  /**
+   * Registers subsystems with the scheduler.  This must be called for the subsystem's periodic
+   * block to run when the scheduler is run, and for the subsystem's default command to be
+   * scheduled.  It is recommended to call this from the constructor of your subsystem
+   * implementations.
+   *
+   * @param subsystems the subsystem to register
+   */
+  public void registerSubsystem(Subsystem... subsystems) {
+    for (Subsystem subsystem : subsystems) {
+      m_subsystems.put(subsystem, null);
+    }
+  }
+
+  /**
+   * Un-registers subsystems with the scheduler.  The subsystem will no longer have its periodic
+   * block called, and will not have its default command scheduled.
+   *
+   * @param subsystems the subsystem to un-register
+   */
+  public void unregisterSubsystem(Subsystem... subsystems) {
+    m_subsystems.keySet().removeAll(Set.of(subsystems));
+  }
+
+  /**
+   * Sets the default command for a subsystem.  Registers that subsystem if it is not already
+   * registered.  Default commands will run whenever there is no other command currently scheduled
+   * that requires the subsystem.  Default commands should be written to never end (i.e. their
+   * {@link Command#isFinished()} method should return false), as they would simply be re-scheduled
+   * if they do.  Default commands must also require their subsystem.
+   *
+   * @param subsystem      the subsystem whose default command will be set
+   * @param defaultCommand the default command to associate with the subsystem
+   */
+  public void setDefaultCommand(Subsystem subsystem, Command defaultCommand) {
+    if (!defaultCommand.getRequirements().contains(subsystem)) {
+      throw new IllegalArgumentException("Default commands must require their subsystem!");
+    }
+
+    if (defaultCommand.isFinished()) {
+      throw new IllegalArgumentException("Default commands should not end!");
+    }
+
+    m_subsystems.put(subsystem, defaultCommand);
+  }
+
+  /**
+   * Gets the default command associated with this subsystem.  Null if this subsystem has no default
+   * command associated with it.
+   *
+   * @param subsystem the subsystem to inquire about
+   * @return the default command associated with the subsystem
+   */
+  public Command getDefaultCommand(Subsystem subsystem) {
+    return m_subsystems.get(subsystem);
+  }
+
+  /**
+   * Cancels commands.  The scheduler will only call the interrupted method of a canceled command,
+   * not the end method (though the interrupted method may itself call the end method).  Commands
+   * will be canceled even if they are not scheduled as interruptible.
+   *
+   * @param commands the commands to cancel
+   */
+  public void cancel(Command... commands) {
+    for (Command command : commands) {
+      if (!m_scheduledCommands.containsKey(command)) {
+        continue;
+      }
+
+      command.end(true);
+      for (Consumer<Command> action : m_interruptActions) {
+        action.accept(command);
+      }
+      m_scheduledCommands.remove(command);
+      m_requirements.keySet().removeAll(command.getRequirements());
+    }
+  }
+
+  /**
+   * Cancels all commands that are currently scheduled.
+   */
+  public void cancelAll() {
+    for (Command command : m_scheduledCommands.keySet()) {
+      cancel(command);
+    }
+  }
+
+  /**
+   * Returns the time since a given command was scheduled.  Note that this only works on commands
+   * that are directly scheduled by the scheduler; it will not work on commands inside of
+   * commandgroups, as the scheduler does not see them.
+   *
+   * @param command the command to query
+   * @return the time since the command was scheduled, in seconds
+   */
+  public double timeSinceScheduled(Command command) {
+    CommandState commandState = m_scheduledCommands.get(command);
+    if (commandState != null) {
+      return commandState.timeSinceInitialized();
+    } else {
+      return -1;
+    }
+  }
+
+  /**
+   * Whether the given commands are running.  Note that this only works on commands that are
+   * directly scheduled by the scheduler; it will not work on commands inside of CommandGroups, as
+   * the scheduler does not see them.
+   *
+   * @param commands the command to query
+   * @return whether the command is currently scheduled
+   */
+  public boolean isScheduled(Command... commands) {
+    return m_scheduledCommands.keySet().containsAll(Set.of(commands));
+  }
+
+  /**
+   * Returns the command currently requiring a given subsystem.  Null if no command is currently
+   * requiring the subsystem
+   *
+   * @param subsystem the subsystem to be inquired about
+   * @return the command currently requiring the subsystem
+   */
+  public Command requiring(Subsystem subsystem) {
+    return m_requirements.get(subsystem);
+  }
+
+  /**
+   * Disables the command scheduler.
+   */
+  public void disable() {
+    m_disabled = true;
+  }
+
+  /**
+   * Enables the command scheduler.
+   */
+  public void enable() {
+    m_disabled = false;
+  }
+
+  /**
+   * Adds an action to perform on the initialization of any command by the scheduler.
+   *
+   * @param action the action to perform
+   */
+  public void onCommandInitialize(Consumer<Command> action) {
+    m_initActions.add(action);
+  }
+
+  /**
+   * Adds an action to perform on the execution of any command by the scheduler.
+   *
+   * @param action the action to perform
+   */
+  public void onCommandExecute(Consumer<Command> action) {
+    m_executeActions.add(action);
+  }
+
+  /**
+   * Adds an action to perform on the interruption of any command by the scheduler.
+   *
+   * @param action the action to perform
+   */
+  public void onCommandInterrupt(Consumer<Command> action) {
+    m_interruptActions.add(action);
+  }
+
+  /**
+   * Adds an action to perform on the finishing of any command by the scheduler.
+   *
+   * @param action the action to perform
+   */
+  public void onCommandFinish(Consumer<Command> action) {
+    m_finishActions.add(action);
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    builder.setSmartDashboardType("Scheduler");
+    m_namesEntry = builder.getEntry("Names");
+    m_idsEntry = builder.getEntry("Ids");
+    m_cancelEntry = builder.getEntry("Cancel");
+    builder.setUpdateTable(() -> {
+
+      if (m_namesEntry == null || m_idsEntry == null || m_cancelEntry == null) {
+        return;
+      }
+
+      Map<Double, Command> ids = new LinkedHashMap<>();
+
+
+      for (Command command : m_scheduledCommands.keySet()) {
+        ids.put((double) command.hashCode(), command);
+      }
+
+      double[] toCancel = m_cancelEntry.getDoubleArray(new double[0]);
+      if (toCancel.length > 0) {
+        for (double hash : toCancel) {
+          cancel(ids.get(hash));
+          ids.remove(hash);
+        }
+        m_cancelEntry.setDoubleArray(new double[0]);
+      }
+
+      List<String> names = new ArrayList<>();
+
+      ids.values().forEach(command -> names.add(command.getName()));
+
+      m_namesEntry.setStringArray(names.toArray(new String[0]));
+      m_idsEntry.setNumberArray(ids.keySet().toArray(new Double[0]));
+    });
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/CommandState.java

@@ -0,0 +1,44 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import edu.wpi.first.wpilibj.Timer;
+
+/**
+ * Class that holds scheduling state for a command.  Used internally by the
+ * {@link CommandScheduler}.
+ */
+class CommandState {
+  //The time since this command was initialized.
+  private double m_startTime = -1;
+
+  //Whether or not it is interruptible.
+  private final boolean m_interruptible;
+
+  CommandState(boolean interruptible) {
+    m_interruptible = interruptible;
+    startTiming();
+    startRunning();
+  }
+
+  private void startTiming() {
+    m_startTime = Timer.getFPGATimestamp();
+  }
+
+  synchronized void startRunning() {
+    m_startTime = -1;
+  }
+
+  boolean isInterruptible() {
+    return m_interruptible;
+  }
+
+  double timeSinceInitialized() {
+    return m_startTime != -1 ? Timer.getFPGATimestamp() - m_startTime : -1;
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ConditionalCommand.java

@@ -0,0 +1,82 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.function.BooleanSupplier;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+import static edu.wpi.first.wpilibj2.command.CommandGroupBase.requireUngrouped;
+
+/**
+ * Runs one of two commands, depending on the value of the given condition when this command is
+ * initialized.  Does not actually schedule the selected command - rather, the command is run
+ * through this command; this ensures that the command will behave as expected if used as part of a
+ * CommandGroup.  Requires the requirements of both commands, again to ensure proper functioning
+ * when used in a CommandGroup.  If this is undesired, consider using {@link ScheduleCommand}.
+ *
+ * <p>As this command contains multiple component commands within it, it is technically a command
+ * group; the command instances that are passed to it cannot be added to any other groups, or
+ * scheduled individually.
+ *
+ * <p>As a rule, CommandGroups require the union of the requirements of their component commands.
+ */
+public class ConditionalCommand extends CommandBase {
+  private final Command m_onTrue;
+  private final Command m_onFalse;
+  private final BooleanSupplier m_condition;
+  private Command m_selectedCommand;
+
+  /**
+   * Creates a new ConditionalCommand.
+   *
+   * @param onTrue    the command to run if the condition is true
+   * @param onFalse   the command to run if the condition is false
+   * @param condition the condition to determine which command to run
+   */
+  public ConditionalCommand(Command onTrue, Command onFalse, BooleanSupplier condition) {
+    requireUngrouped(onTrue, onFalse);
+
+    CommandGroupBase.registerGroupedCommands(onTrue, onFalse);
+
+    m_onTrue = onTrue;
+    m_onFalse = onFalse;
+    m_condition = requireNonNullParam(condition, "condition", "ConditionalCommand");
+    m_requirements.addAll(m_onTrue.getRequirements());
+    m_requirements.addAll(m_onFalse.getRequirements());
+  }
+
+  @Override
+  public void initialize() {
+    if (m_condition.getAsBoolean()) {
+      m_selectedCommand = m_onTrue;
+    } else {
+      m_selectedCommand = m_onFalse;
+    }
+    m_selectedCommand.initialize();
+  }
+
+  @Override
+  public void execute() {
+    m_selectedCommand.execute();
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    m_selectedCommand.end(interrupted);
+  }
+
+  @Override
+  public boolean isFinished() {
+    return m_selectedCommand.isFinished();
+  }
+
+  @Override
+  public boolean runsWhenDisabled() {
+    return m_onTrue.runsWhenDisabled() && m_onFalse.runsWhenDisabled();
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/FunctionalCommand.java

@@ -0,0 +1,65 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.function.BooleanSupplier;
+import java.util.function.Consumer;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * A command that allows the user to pass in functions for each of the basic command methods through
+ * the constructor.  Useful for inline definitions of complex commands - note, however, that if a
+ * command is beyond a certain complexity it is usually better practice to write a proper class for
+ * it than to inline it.
+ */
+public class FunctionalCommand extends CommandBase {
+  protected final Runnable m_onInit;
+  protected final Runnable m_onExecute;
+  protected final Consumer<Boolean> m_onEnd;
+  protected final BooleanSupplier m_isFinished;
+
+  /**
+   * Creates a new FunctionalCommand.
+   *
+   * @param onInit       the function to run on command initialization
+   * @param onExecute    the function to run on command execution
+   * @param onEnd        the function to run on command end
+   * @param isFinished   the function that determines whether the command has finished
+   * @param requirements the subsystems required by this command
+   */
+  public FunctionalCommand(Runnable onInit, Runnable onExecute, Consumer<Boolean> onEnd,
+                           BooleanSupplier isFinished, Subsystem... requirements) {
+    m_onInit = requireNonNullParam(onInit, "onInit", "FunctionalCommand");
+    m_onExecute = requireNonNullParam(onExecute, "onExecute", "FunctionalCommand");
+    m_onEnd = requireNonNullParam(onEnd, "onEnd", "FunctionalCommand");
+    m_isFinished = requireNonNullParam(isFinished, "isFinished", "FunctionalCommand");
+
+    addRequirements(requirements);
+  }
+
+  @Override
+  public void initialize() {
+    m_onInit.run();
+  }
+
+  @Override
+  public void execute() {
+    m_onExecute.run();
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    m_onEnd.accept(interrupted);
+  }
+
+  @Override
+  public boolean isFinished() {
+    return m_isFinished.getAsBoolean();
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/InstantCommand.java

@@ -0,0 +1,50 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * A Command that runs instantly; it will initialize, execute once, and end on the same
+ * iteration of the scheduler.  Users can either pass in a Runnable and a set of requirements,
+ * or else subclass this command if desired.
+ */
+public class InstantCommand extends CommandBase {
+  private final Runnable m_toRun;
+
+  /**
+   * Creates a new InstantCommand that runs the given Runnable with the given requirements.
+   *
+   * @param toRun        the Runnable to run
+   * @param requirements the subsystems required by this command
+   */
+  public InstantCommand(Runnable toRun, Subsystem... requirements) {
+    m_toRun = requireNonNullParam(toRun, "toRun", "InstantCommand");
+
+    addRequirements(requirements);
+  }
+
+  /**
+   * Creates a new InstantCommand with a Runnable that does nothing.  Useful only as a no-arg
+   * constructor to call implicitly from subclass constructors.
+   */
+  public InstantCommand() {
+    m_toRun = () -> {
+    };
+  }
+
+  @Override
+  public void initialize() {
+    m_toRun.run();
+  }
+
+  @Override
+  public final boolean isFinished() {
+    return true;
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/NotifierCommand.java

@@ -0,0 +1,49 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.function.BooleanSupplier;
+
+import edu.wpi.first.wpilibj.Notifier;
+
+/**
+ * A command that starts a notifier to run the given runnable periodically in a separate thread.
+ * Has no end condition as-is; either subclass it or use {@link Command#withTimeout(double)} or
+ * {@link Command#interruptOn(BooleanSupplier)} to give it one.
+ *
+ * <p>WARNING: Do not use this class unless you are confident in your ability to make the executed
+ * code thread-safe.  If you do not know what "thread-safe" means, that is a good sign that
+ * you should not use this class.
+ */
+public class NotifierCommand extends CommandBase {
+  protected final Notifier m_notifier;
+  protected final double m_period;
+
+  /**
+   * Creates a new NotifierCommand.
+   *
+   * @param toRun        the runnable for the notifier to run
+   * @param period       the period at which the notifier should run, in seconds
+   * @param requirements the subsystems required by this command
+   */
+  public NotifierCommand(Runnable toRun, double period, Subsystem... requirements) {
+    m_notifier = new Notifier(toRun);
+    m_period = period;
+    addRequirements(requirements);
+  }
+
+  @Override
+  public void initialize() {
+    m_notifier.startPeriodic(m_period);
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    m_notifier.stop();
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/PIDCommand.java

@@ -0,0 +1,155 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.Set;
+import java.util.function.DoubleConsumer;
+import java.util.function.DoubleSupplier;
+
+import edu.wpi.first.wpilibj.controller.PIDController;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * A command that controls an output with a {@link PIDController}.  Runs forever by default - to add
+ * exit conditions and/or other behavior, subclass this class.  The controller calculation and
+ * output are performed synchronously in the command's execute() method.
+ */
+public class PIDCommand extends CommandBase {
+  protected final PIDController m_controller;
+  protected DoubleSupplier m_measurement;
+  protected DoubleSupplier m_setpoint;
+  protected DoubleConsumer m_useOutput;
+
+  /**
+   * Creates a new PIDCommand, which controls the given output with a PIDController.
+   *
+   * @param controller        the controller that controls the output.
+   * @param measurementSource the measurement of the process variable
+   * @param setpointSource    the controller's setpoint
+   * @param useOutput         the controller's output
+   * @param requirements      the subsystems required by this command
+   */
+  public PIDCommand(PIDController controller, DoubleSupplier measurementSource,
+                    DoubleSupplier setpointSource, DoubleConsumer useOutput,
+                    Subsystem... requirements) {
+    requireNonNullParam(controller, "controller", "SynchronousPIDCommand");
+    requireNonNullParam(measurementSource, "measurementSource", "SynchronousPIDCommand");
+    requireNonNullParam(setpointSource, "setpointSource", "SynchronousPIDCommand");
+    requireNonNullParam(useOutput, "useOutput", "SynchronousPIDCommand");
+
+    m_controller = controller;
+    m_useOutput = useOutput;
+    m_measurement = measurementSource;
+    m_setpoint = setpointSource;
+    m_requirements.addAll(Set.of(requirements));
+  }
+
+  /**
+   * Creates a new PIDCommand, which controls the given output with a PIDController.
+   *
+   * @param controller        the controller that controls the output.
+   * @param measurementSource the measurement of the process variable
+   * @param setpoint          the controller's setpoint
+   * @param useOutput         the controller's output
+   * @param requirements      the subsystems required by this command
+   */
+  public PIDCommand(PIDController controller, DoubleSupplier measurementSource,
+                    double setpoint, DoubleConsumer useOutput,
+                    Subsystem... requirements) {
+    this(controller, measurementSource, () -> setpoint, useOutput, requirements);
+  }
+
+  @Override
+  public void initialize() {
+    m_controller.reset();
+  }
+
+  @Override
+  public void execute() {
+    useOutput(m_controller.calculate(getMeasurement(), getSetpoint()));
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    useOutput(0);
+  }
+
+  /**
+   * Sets the function that uses the output of the PIDController.
+   *
+   * @param useOutput The function that uses the output.
+   */
+  public final void setOutput(DoubleConsumer useOutput) {
+    m_useOutput = useOutput;
+  }
+
+  /**
+   * Returns the PIDController used by the command.
+   *
+   * @return The PIDController
+   */
+  public PIDController getController() {
+    return m_controller;
+  }
+
+  /**
+   * Sets the setpoint for the controller to track the given source.
+   *
+   * @param setpointSource The setpoint source
+   */
+  public void setSetpoint(DoubleSupplier setpointSource) {
+    m_setpoint = setpointSource;
+  }
+
+  /**
+   * Sets the setpoint for the controller to a constant value.
+   *
+   * @param setpoint The setpoint
+   */
+  public void setSetpoint(double setpoint) {
+    setSetpoint(() -> setpoint);
+  }
+
+  /**
+   * Sets the setpoint for the controller to a constant value relative (i.e. added to) the current
+   * setpoint.
+   *
+   * @param relativeReference The change in setpoint
+   */
+  public void setSetpointRelative(double relativeReference) {
+    setSetpoint(m_controller.getSetpoint() + relativeReference);
+  }
+
+  /**
+   * Gets the setpoint for the controller.  Wraps the passed-in function for readability.
+   *
+   * @return The setpoint for the controller
+   */
+  private double getSetpoint() {
+    return m_setpoint.getAsDouble();
+  }
+
+  /**
+   * Gets the measurement of the process variable. Wraps the passed-in function for readability.
+   *
+   * @return The measurement of the process variable
+   */
+  private double getMeasurement() {
+    return m_measurement.getAsDouble();
+  }
+
+  /**
+   * Uses the output of the controller.  Wraps the passed-in function for readability.
+   *
+   * @param output The output value to use
+   */
+  private void useOutput(double output) {
+    m_useOutput.accept(output);
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/PIDSubsystem.java

@@ -0,0 +1,79 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import edu.wpi.first.wpilibj.controller.PIDController;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * A subsystem that uses a {@link PIDController} to control an output.  The controller is run
+ * synchronously from the subsystem's periodic() method.
+ */
+public abstract class PIDSubsystem extends SubsystemBase {
+  protected final PIDController m_controller;
+  protected boolean m_enabled;
+
+  /**
+   * Creates a new PIDSubsystem.
+   *
+   * @param controller the PIDController to use
+   */
+  public PIDSubsystem(PIDController controller) {
+    requireNonNullParam(controller, "controller", "PIDSubsystem");
+    m_controller = controller;
+  }
+
+  @Override
+  public void periodic() {
+    if (m_enabled) {
+      useOutput(m_controller.calculate(getMeasurement(), getSetpoint()));
+    }
+  }
+
+  public PIDController getController() {
+    return m_controller;
+  }
+
+  /**
+   * Uses the output from the PIDController.
+   *
+   * @param output the output of the PIDController
+   */
+  public abstract void useOutput(double output);
+
+  /**
+   * Returns the reference (setpoint) used by the PIDController.
+   *
+   * @return the reference (setpoint) to be used by the controller
+   */
+  public abstract double getSetpoint();
+
+  /**
+   * Returns the measurement of the process variable used by the PIDController.
+   *
+   * @return the measurement of the process variable
+   */
+  public abstract double getMeasurement();
+
+  /**
+   * Enables the PID control.  Resets the controller.
+   */
+  public void enable() {
+    m_enabled = true;
+    m_controller.reset();
+  }
+
+  /**
+   * Disables the PID control.  Sets output to zero.
+   */
+  public void disable() {
+    m_enabled = false;
+    useOutput(0);
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroup.java

@@ -0,0 +1,99 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.Collections;
+import java.util.HashMap;
+import java.util.Map;
+
+/**
+ * A CommandGroup that runs a set of commands in parallel, ending when the last command ends.
+ *
+ * <p>As a rule, CommandGroups require the union of the requirements of their component commands.
+ */
+public class ParallelCommandGroup extends CommandGroupBase {
+  //maps commands in this group to whether they are still running
+  private final Map<Command, Boolean> m_commands = new HashMap<>();
+  private boolean m_runWhenDisabled = true;
+
+  /**
+   * Creates a new ParallelCommandGroup.  The given commands will be executed simultaneously.
+   * The command group will finish when the last command finishes.  If the CommandGroup is
+   * interrupted, only the commands that are still running will be interrupted.
+   *
+   * @param commands the commands to include in this group.
+   */
+  public ParallelCommandGroup(Command... commands) {
+    addCommands(commands);
+  }
+
+  @Override
+  public final void addCommands(Command... commands) {
+    requireUngrouped(commands);
+
+    if (m_commands.containsValue(true)) {
+      throw new IllegalStateException(
+          "Commands cannot be added to a CommandGroup while the group is running");
+    }
+
+    registerGroupedCommands(commands);
+
+    for (Command command : commands) {
+      if (!Collections.disjoint(command.getRequirements(), m_requirements)) {
+        throw new IllegalArgumentException("Multiple commands in a parallel group cannot"
+            + "require the same subsystems");
+      }
+      m_commands.put(command, false);
+      m_requirements.addAll(command.getRequirements());
+      m_runWhenDisabled &= command.runsWhenDisabled();
+    }
+  }
+
+  @Override
+  public void initialize() {
+    for (Map.Entry<Command, Boolean> commandRunning : m_commands.entrySet()) {
+      commandRunning.getKey().initialize();
+      commandRunning.setValue(true);
+    }
+  }
+
+  @Override
+  public void execute() {
+    for (Map.Entry<Command, Boolean> commandRunning : m_commands.entrySet()) {
+      if (!commandRunning.getValue()) {
+        continue;
+      }
+      commandRunning.getKey().execute();
+      if (commandRunning.getKey().isFinished()) {
+        commandRunning.getKey().end(false);
+        commandRunning.setValue(false);
+      }
+    }
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    if (interrupted) {
+      for (Map.Entry<Command, Boolean> commandRunning : m_commands.entrySet()) {
+        if (commandRunning.getValue()) {
+          commandRunning.getKey().end(true);
+        }
+      }
+    }
+  }
+
+  @Override
+  public boolean isFinished() {
+    return !m_commands.values().contains(true);
+  }
+
+  @Override
+  public boolean runsWhenDisabled() {
+    return m_runWhenDisabled;
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroup.java

@@ -0,0 +1,118 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.Collections;
+import java.util.HashMap;
+import java.util.Map;
+
+/**
+ * A CommandGroup that runs a set of commands in parallel, ending only when a specific command
+ * (the "deadline") ends, interrupting all other commands that are still running at that point.
+ *
+ * <p>As a rule, CommandGroups require the union of the requirements of their component commands.
+ */
+public class ParallelDeadlineGroup extends CommandGroupBase {
+  //maps commands in this group to whether they are still running
+  private final Map<Command, Boolean> m_commands = new HashMap<>();
+  private boolean m_runWhenDisabled = true;
+  private Command m_deadline;
+
+  /**
+   * Creates a new ParallelDeadlineGroup.  The given commands (including the deadline) will be
+   * executed simultaneously.  The CommandGroup will finish when the deadline finishes,
+   * interrupting all other still-running commands.  If the CommandGroup is interrupted, only
+   * the commands still running will be interrupted.
+   *
+   * @param deadline the command that determines when the group ends
+   * @param commands the commands to be executed
+   */
+  public ParallelDeadlineGroup(Command deadline, Command... commands) {
+    m_deadline = deadline;
+    addCommands(commands);
+    if (!m_commands.containsKey(deadline)) {
+      addCommands(deadline);
+    }
+  }
+
+  /**
+   * Sets the deadline to the given command.  The deadline is added to the group if it is not
+   * already contained.
+   *
+   * @param deadline the command that determines when the group ends
+   */
+  public void setDeadline(Command deadline) {
+    if (!m_commands.containsKey(deadline)) {
+      addCommands(deadline);
+    }
+    m_deadline = deadline;
+  }
+
+  @Override
+  public final void addCommands(Command... commands) {
+    requireUngrouped(commands);
+
+    if (m_commands.containsValue(true)) {
+      throw new IllegalStateException(
+          "Commands cannot be added to a CommandGroup while the group is running");
+    }
+
+    registerGroupedCommands(commands);
+
+    for (Command command : commands) {
+      if (!Collections.disjoint(command.getRequirements(), m_requirements)) {
+        throw new IllegalArgumentException("Multiple commands in a parallel group cannot"
+            + "require the same subsystems");
+      }
+      m_commands.put(command, false);
+      m_requirements.addAll(command.getRequirements());
+      m_runWhenDisabled &= command.runsWhenDisabled();
+    }
+  }
+
+  @Override
+  public void initialize() {
+    for (Map.Entry<Command, Boolean> commandRunning : m_commands.entrySet()) {
+      commandRunning.getKey().initialize();
+      commandRunning.setValue(true);
+    }
+  }
+
+  @Override
+  public void execute() {
+    for (Map.Entry<Command, Boolean> commandRunning : m_commands.entrySet()) {
+      if (!commandRunning.getValue()) {
+        continue;
+      }
+      commandRunning.getKey().execute();
+      if (commandRunning.getKey().isFinished()) {
+        commandRunning.getKey().end(false);
+        commandRunning.setValue(false);
+      }
+    }
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    for (Map.Entry<Command, Boolean> commandRunning : m_commands.entrySet()) {
+      if (commandRunning.getValue()) {
+        commandRunning.getKey().end(true);
+      }
+    }
+  }
+
+  @Override
+  public boolean isFinished() {
+    return m_deadline.isFinished();
+  }
+
+  @Override
+  public boolean runsWhenDisabled() {
+    return m_runWhenDisabled;
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroup.java

@@ -0,0 +1,95 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.Collections;
+import java.util.HashSet;
+import java.util.Set;
+
+/**
+ * A CommandGroup that runs a set of commands in parallel, ending when any one of the commands ends
+ * and interrupting all the others.
+ *
+ * <p>As a rule, CommandGroups require the union of the requirements of their component commands.
+ */
+public class ParallelRaceGroup extends CommandGroupBase {
+  private final Set<Command> m_commands = new HashSet<>();
+  private boolean m_runWhenDisabled = true;
+  private boolean m_finished = true;
+
+  /**
+   * Creates a new ParallelCommandRace.  The given commands will be executed simultaneously, and
+   * will "race to the finish" - the first command to finish ends the entire command, with all other
+   * commands being interrupted.
+   *
+   * @param commands the commands to include in this group.
+   */
+  public ParallelRaceGroup(Command... commands) {
+    addCommands(commands);
+  }
+
+  @Override
+  public final void addCommands(Command... commands) {
+    requireUngrouped(commands);
+
+    if (!m_finished) {
+      throw new IllegalStateException(
+          "Commands cannot be added to a CommandGroup while the group is running");
+    }
+
+    registerGroupedCommands(commands);
+
+    for (Command command : commands) {
+      if (!Collections.disjoint(command.getRequirements(), m_requirements)) {
+        throw new IllegalArgumentException("Multiple commands in a parallel group cannot"
+            + " require the same subsystems");
+      }
+      m_commands.add(command);
+      m_requirements.addAll(command.getRequirements());
+      m_runWhenDisabled &= command.runsWhenDisabled();
+    }
+  }
+
+  @Override
+  public void initialize() {
+    m_finished = false;
+    for (Command command : m_commands) {
+      command.initialize();
+    }
+  }
+
+  @Override
+  public void execute() {
+    for (Command command : m_commands) {
+      command.execute();
+      if (command.isFinished()) {
+        m_finished = true;
+        command.end(false);
+      }
+    }
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    for (Command command : m_commands) {
+      if (!command.isFinished()) {
+        command.end(true);
+      }
+    }
+  }
+
+  @Override
+  public boolean isFinished() {
+    return m_finished;
+  }
+
+  @Override
+  public boolean runsWhenDisabled() {
+    return m_runWhenDisabled;
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/PerpetualCommand.java

@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import static edu.wpi.first.wpilibj2.command.CommandGroupBase.registerGroupedCommands;
+import static edu.wpi.first.wpilibj2.command.CommandGroupBase.requireUngrouped;
+
+/**
+ * A command that runs another command in perpetuity, ignoring that command's end conditions.  While
+ * this class does not extend {@link CommandGroupBase}, it is still considered a CommandGroup, as it
+ * allows one to compose another command within it; the command instances that are passed to it
+ * cannot be added to any other groups, or scheduled individually.
+ *
+ * <p>As a rule, CommandGroups require the union of the requirements of their component commands.
+ */
+public class PerpetualCommand extends CommandBase {
+  protected final Command m_command;
+
+  /**
+   * Creates a new PerpetualCommand.  Will run another command in perpetuity, ignoring that
+   * command's end conditions, unless this command itself is interrupted.
+   *
+   * @param command the command to run perpetually
+   */
+  public PerpetualCommand(Command command) {
+    requireUngrouped(command);
+    registerGroupedCommands(command);
+    m_command = command;
+    m_requirements.addAll(command.getRequirements());
+  }
+
+  @Override
+  public void initialize() {
+    m_command.initialize();
+  }
+
+  @Override
+  public void execute() {
+    m_command.execute();
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    m_command.end(interrupted);
+  }
+
+  @Override
+  public boolean runsWhenDisabled() {
+    return m_command.runsWhenDisabled();
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/PrintCommand.java

@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+/**
+ * A command that prints a string when initialized.
+ */
+public class PrintCommand extends InstantCommand {
+  /**
+   * Creates a new a PrintCommand.
+   *
+   * @param message the message to print
+   */
+  public PrintCommand(String message) {
+    super(() -> System.out.println(message));
+  }
+
+  @Override
+  public boolean runsWhenDisabled() {
+    return true;
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ProxyScheduleCommand.java

@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.Set;
+
+/**
+ * Schedules the given commands when this command is initialized, and ends when all the commands are
+ * no longer scheduled.  Useful for forking off from CommandGroups.  If this command is interrupted,
+ * it will cancel all of the commands.
+ */
+public class ProxyScheduleCommand extends CommandBase {
+  private final Set<Command> m_toSchedule;
+  private boolean m_finished;
+
+  /**
+   * Creates a new ProxyScheduleCommand that schedules the given commands when initialized,
+   * and ends when they are all no longer scheduled.
+   *
+   * @param toSchedule the commands to schedule
+   */
+  public ProxyScheduleCommand(Command... toSchedule) {
+    m_toSchedule = Set.of(toSchedule);
+  }
+
+  @Override
+  public void initialize() {
+    for (Command command : m_toSchedule) {
+      command.schedule();
+    }
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    if (interrupted) {
+      for (Command command : m_toSchedule) {
+        command.cancel();
+      }
+    }
+  }
+
+  @Override
+  public void execute() {
+    m_finished = true;
+    for (Command command : m_toSchedule) {
+      m_finished &= !command.isScheduled();
+    }
+  }
+
+  @Override
+  public boolean isFinished() {
+    return m_finished;
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/RunCommand.java

@@ -0,0 +1,39 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.function.BooleanSupplier;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * A command that runs a Runnable continuously.  Has no end condition as-is;
+ * either subclass it or use {@link Command#withTimeout(double)} or
+ * {@link Command#interruptOn(BooleanSupplier)} to give it one.  If you only wish
+ * to execute a Runnable once, use {@link InstantCommand}.
+ */
+public class RunCommand extends CommandBase {
+  protected final Runnable m_toRun;
+
+  /**
+   * Creates a new RunCommand.  The Runnable will be run continuously until the command
+   * ends.  Does not run when disabled.
+   *
+   * @param toRun        the Runnable to run
+   * @param requirements the subsystems to require
+   */
+  public RunCommand(Runnable toRun, Subsystem... requirements) {
+    m_toRun = requireNonNullParam(toRun, "toRun", "RunCommand");
+    addRequirements(requirements);
+  }
+
+  @Override
+  public void execute() {
+    m_toRun.run();
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ScheduleCommand.java

@@ -0,0 +1,45 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.Set;
+
+/**
+ * Schedules the given commands when this command is initialized.  Useful for forking off from
+ * CommandGroups.  Note that if run from a CommandGroup, the group will not know about the status
+ * of the scheduled commands, and will treat this command as finishing instantly.
+ */
+public class ScheduleCommand extends CommandBase {
+  private final Set<Command> m_toSchedule;
+
+  /**
+   * Creates a new ScheduleCommand that schedules the given commands when initialized.
+   *
+   * @param toSchedule the commands to schedule
+   */
+  public ScheduleCommand(Command... toSchedule) {
+    m_toSchedule = Set.of(toSchedule);
+  }
+
+  @Override
+  public void initialize() {
+    for (Command command : m_toSchedule) {
+      command.schedule();
+    }
+  }
+
+  @Override
+  public boolean isFinished() {
+    return true;
+  }
+
+  @Override
+  public boolean runsWhenDisabled() {
+    return true;
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/SelectCommand.java

@@ -0,0 +1,110 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.Map;
+import java.util.function.Supplier;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+import static edu.wpi.first.wpilibj2.command.CommandGroupBase.requireUngrouped;
+
+/**
+ * Runs one of a selection of commands, either using a selector and a key to command mapping, or a
+ * supplier that returns the command directly at runtime.  Does not actually schedule the selected
+ * command - rather, the command is run through this command; this ensures that the command will
+ * behave as expected if used as part of a CommandGroup.  Requires the requirements of all included
+ * commands, again to ensure proper functioning when used in a CommandGroup.  If this is undesired,
+ * consider using {@link ScheduleCommand}.
+ *
+ * <p>As this command contains multiple component commands within it, it is technically a command
+ * group; the command instances that are passed to it cannot be added to any other groups, or
+ * scheduled individually.
+ *
+ * <p>As a rule, CommandGroups require the union of the requirements of their component commands.
+ */
+public class SelectCommand extends CommandBase {
+  private final Map<Object, Command> m_commands;
+  private final Supplier<Object> m_selector;
+  private final Supplier<Command> m_toRun;
+  private Command m_selectedCommand;
+
+  /**
+   * Creates a new selectcommand.
+   *
+   * @param commands the map of commands to choose from
+   * @param selector the selector to determine which command to run
+   */
+  public SelectCommand(Map<Object, Command> commands, Supplier<Object> selector) {
+    requireUngrouped(commands.values());
+
+    CommandGroupBase.registerGroupedCommands(commands.values().toArray(new Command[]{}));
+
+    m_commands = requireNonNullParam(commands, "commands", "SelectCommand");
+    m_selector = requireNonNullParam(selector, "selector", "SelectCommand");
+
+    m_toRun = null;
+
+    for (Command command : m_commands.values()) {
+      m_requirements.addAll(command.getRequirements());
+    }
+  }
+
+  /**
+   * Creates a new selectcommand.
+   *
+   * @param toRun a supplier providing the command to run
+   */
+  public SelectCommand(Supplier<Command> toRun) {
+    m_commands = null;
+    m_selector = null;
+    m_toRun = requireNonNullParam(toRun, "toRun", "SelectCommand");
+  }
+
+  @Override
+  public void initialize() {
+    if (m_selector != null) {
+      if (!m_commands.keySet().contains(m_selector.get())) {
+        m_selectedCommand = new PrintCommand(
+            "SelectCommand selector value does not correspond to" + " any command!");
+        return;
+      }
+      m_selectedCommand = m_commands.get(m_selector.get());
+    } else {
+      m_selectedCommand = m_toRun.get();
+    }
+    m_selectedCommand.initialize();
+  }
+
+  @Override
+  public void execute() {
+    m_selectedCommand.execute();
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    m_selectedCommand.end(interrupted);
+  }
+
+  @Override
+  public boolean isFinished() {
+    return m_selectedCommand.isFinished();
+  }
+
+  @Override
+  public boolean runsWhenDisabled() {
+    if (m_commands != null) {
+      boolean runsWhenDisabled = true;
+      for (Command command : m_commands.values()) {
+        runsWhenDisabled &= command.runsWhenDisabled();
+      }
+      return runsWhenDisabled;
+    } else {
+      return m_toRun.get().runsWhenDisabled();
+    }
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroup.java

@@ -0,0 +1,95 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.ArrayList;
+import java.util.List;
+
+/**
+ * A CommandGroups that runs a list of commands in sequence.
+ *
+ * <p>As a rule, CommandGroups require the union of the requirements of their component commands.
+ */
+public class SequentialCommandGroup extends CommandGroupBase {
+  private final List<Command> m_commands = new ArrayList<>();
+  private int m_currentCommandIndex = -1;
+  private boolean m_runWhenDisabled = true;
+
+  /**
+   * Creates a new SequentialCommandGroup.  The given commands will be run sequentially, with
+   * the CommandGroup finishing when the last command finishes.
+   *
+   * @param commands the commands to include in this group.
+   */
+  public SequentialCommandGroup(Command... commands) {
+    addCommands(commands);
+  }
+
+  @Override
+  public final void addCommands(Command... commands) {
+    requireUngrouped(commands);
+
+    if (m_currentCommandIndex != -1) {
+      throw new IllegalStateException(
+          "Commands cannot be added to a CommandGroup while the group is running");
+    }
+
+    registerGroupedCommands(commands);
+
+    for (Command command : commands) {
+      m_commands.add(command);
+      m_requirements.addAll(command.getRequirements());
+      m_runWhenDisabled &= command.runsWhenDisabled();
+    }
+  }
+
+  @Override
+  public void initialize() {
+    m_currentCommandIndex = 0;
+
+    if (!m_commands.isEmpty()) {
+      m_commands.get(0).initialize();
+    }
+  }
+
+  @Override
+  public void execute() {
+    if (m_commands.isEmpty()) {
+      return;
+    }
+
+    Command currentCommand = m_commands.get(m_currentCommandIndex);
+
+    currentCommand.execute();
+    if (currentCommand.isFinished()) {
+      currentCommand.end(false);
+      m_currentCommandIndex++;
+      if (m_currentCommandIndex < m_commands.size()) {
+        m_commands.get(m_currentCommandIndex).initialize();
+      }
+    }
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    if (interrupted && !m_commands.isEmpty()) {
+      m_commands.get(m_currentCommandIndex).end(true);
+    }
+    m_currentCommandIndex = -1;
+  }
+
+  @Override
+  public boolean isFinished() {
+    return m_currentCommandIndex == m_commands.size();
+  }
+
+  @Override
+  public boolean runsWhenDisabled() {
+    return m_runWhenDisabled;
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/StartEndCommand.java

@@ -0,0 +1,48 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.function.BooleanSupplier;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * A command that runs a given runnable when it is initalized, and another runnable when it ends.
+ * Useful for running and then stopping a motor, or extending and then retracting a solenoid.
+ * Has no end condition as-is; either subclass it or use {@link Command#withTimeout(double)} or
+ * {@link Command#interruptOn(BooleanSupplier)} to give it one.
+ */
+public class StartEndCommand extends CommandBase {
+  protected final Runnable m_onInit;
+  protected final Runnable m_onEnd;
+
+  /**
+   * Creates a new StartEndCommand.  Will run the given runnables when the command starts and when
+   * it ends.
+   *
+   * @param onInit       the Runnable to run on command init
+   * @param onEnd        the Runnable to run on command end
+   * @param requirements the subsystems required by this command
+   */
+  public StartEndCommand(Runnable onInit, Runnable onEnd, Subsystem... requirements) {
+    m_onInit = requireNonNullParam(onInit, "onInit", "StartEndCommand");
+    m_onEnd = requireNonNullParam(onEnd, "onEnd", "StartEndCommand");
+
+    addRequirements(requirements);
+  }
+
+  @Override
+  public void initialize() {
+    m_onInit.run();
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    m_onEnd.run();
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/Subsystem.java

@@ -0,0 +1,76 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+/**
+ * A robot subsystem.  Subsystems are the basic unit of robot organization in the Command-based
+ * framework; they encapsulate low-level hardware objects (motor controllers, sensors, etc) and
+ * provide methods through which they can be used by {@link Command}s.  Subsystems are used by the
+ * {@link CommandScheduler}'s resource management system to ensure multiple robot actions are not
+ * "fighting" over the same hardware; Commands that use a subsystem should include that subsystem
+ * in their {@link Command#getRequirements()} method, and resources used within a subsystem should
+ * generally remain encapsulated and not be shared by other parts of the robot.
+ *
+ * <p>Subsystems must be registered with the scheduler with the
+ * {@link CommandScheduler#registerSubsystem(Subsystem...)} method in order for the
+ * {@link Subsystem#periodic()} method to be called.  It is recommended that this method be called
+ * from the constructor of users' Subsystem implementations.  The {@link SubsystemBase}
+ * class offers a simple base for user implementations that handles this.
+ */
+public interface Subsystem {
+
+  /**
+   * This method is called periodically by the {@link CommandScheduler}.  Useful for updating
+   * subsystem-specific state that you don't want to offload to a {@link Command}.  Teams should
+   * try to be consistent within their own codebases about which responsibilities will be handled
+   * by Commands, and which will be handled here.
+   */
+  default void periodic() {
+  }
+
+  /**
+   * Sets the default {@link Command} of the subsystem.  The default command will be
+   * automatically scheduled when no other commands are scheduled that require the subsystem.
+   * Default commands should generally not end on their own, i.e. their {@link Command#isFinished()}
+   * method should always return false.  Will automatically register this subsystem with the
+   * {@link CommandScheduler}.
+   *
+   * @param defaultCommand the default command to associate with this subsystem
+   */
+  default void setDefaultCommand(Command defaultCommand) {
+    CommandScheduler.getInstance().setDefaultCommand(this, defaultCommand);
+  }
+
+  /**
+   * Gets the default command for this subsystem.  Returns null if no default command is
+   * currently associated with the subsystem.
+   *
+   * @return the default command associated with this subsystem
+   */
+  default Command getDefaultCommand() {
+    return CommandScheduler.getInstance().getDefaultCommand(this);
+  }
+
+  /**
+   * Returns the command currently running on this subsystem.  Returns null if no command is
+   * currently scheduled that requires this subsystem.
+   *
+   * @return the scheduled command currently requiring this subsystem
+   */
+  default Command getCurrentCommand() {
+    return CommandScheduler.getInstance().requiring(this);
+  }
+
+  /**
+   * Registers this subsystem with the {@link CommandScheduler}, allowing its
+   * {@link Subsystem#periodic()} method to be called when the scheduler runs.
+   */
+  default void register() {
+    CommandScheduler.getInstance().registerSubsystem(this);
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/SubsystemBase.java

@@ -0,0 +1,69 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import edu.wpi.first.wpilibj.Sendable;
+import edu.wpi.first.wpilibj.livewindow.LiveWindow;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+
+/**
+ * A base for subsystems that handles registration in the constructor, and provides a more intuitive
+ * method for setting the default command.
+ */
+public abstract class SubsystemBase implements Subsystem, Sendable {
+
+  protected String m_name = this.getClass().getSimpleName();
+
+  public SubsystemBase() {
+    CommandScheduler.getInstance().registerSubsystem(this);
+  }
+
+  @Override
+  public String getName() {
+    return m_name;
+  }
+
+  @Override
+  public void setName(String name) {
+    m_name = name;
+  }
+
+  @Override
+  public String getSubsystem() {
+    return getName();
+  }
+
+  @Override
+  public void setSubsystem(String subsystem) {
+    setName(subsystem);
+  }
+
+  /**
+   * Associates a {@link Sendable} with this Subsystem.
+   * Also update the child's name.
+   *
+   * @param name name to give child
+   * @param child sendable
+   */
+  public void addChild(String name, Sendable child) {
+    child.setName(getSubsystem(), name);
+    LiveWindow.add(child);
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    builder.setSmartDashboardType("Subsystem");
+
+    builder.addBooleanProperty(".hasDefault", () -> getDefaultCommand() != null, null);
+    builder.addStringProperty(".default",
+        () -> getDefaultCommand() != null ? getDefaultCommand().getName() : "none", null);
+    builder.addBooleanProperty(".hasCommand", () -> getCurrentCommand() != null, null);
+    builder.addStringProperty(".command",
+        () -> getCurrentCommand() != null ? getCurrentCommand().getName() : "none", null);
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/WaitCommand.java

@@ -0,0 +1,50 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import edu.wpi.first.wpilibj.Timer;
+
+/**
+ * A command that does nothing but takes a specified amount of time to finish.  Useful for
+ * CommandGroups.  Can also be subclassed to make a command with an internal timer.
+ */
+public class WaitCommand extends CommandBase {
+  protected Timer m_timer = new Timer();
+  private final double m_duration;
+
+  /**
+   * Creates a new WaitCommand.  This command will do nothing, and end after the specified duration.
+   *
+   * @param seconds the time to wait, in seconds
+   */
+  public WaitCommand(double seconds) {
+    m_duration = seconds;
+    setName(m_name + ": " + seconds + " seconds");
+  }
+
+  @Override
+  public void initialize() {
+    m_timer.reset();
+    m_timer.start();
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    m_timer.stop();
+  }
+
+  @Override
+  public boolean isFinished() {
+    return m_timer.hasPeriodPassed(m_duration);
+  }
+
+  @Override
+  public boolean runsWhenDisabled() {
+    return true;
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/WaitUntilCommand.java

@@ -0,0 +1,55 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+
+import java.util.function.BooleanSupplier;
+
+import edu.wpi.first.wpilibj.Timer;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * A command that does nothing but ends after a specified match time or condition.  Useful for
+ * CommandGroups.
+ */
+public class WaitUntilCommand extends CommandBase {
+  private final BooleanSupplier m_condition;
+
+  /**
+   * Creates a new WaitUntilCommand that ends after a given condition becomes true.
+   *
+   * @param condition the condition to determine when to end
+   */
+  public WaitUntilCommand(BooleanSupplier condition) {
+    m_condition = requireNonNullParam(condition, "condition", "WaitUntilCommand");
+  }
+
+  /**
+   * Creates a new WaitUntilCommand that ends after a given match time.
+   *
+   * <p>NOTE: The match timer used for this command is UNOFFICIAL.  Using this command does NOT
+   * guarantee that the time at which the action is performed will be judged to be legal by the
+   * referees.  When in doubt, add a safety factor or time the action manually.
+   *
+   * @param time the match time after which to end, in seconds
+   */
+  public WaitUntilCommand(double time) {
+    this(() -> Timer.getMatchTime() - time > 0);
+  }
+
+  @Override
+  public boolean isFinished() {
+    return m_condition.getAsBoolean();
+  }
+
+  @Override
+  public boolean runsWhenDisabled() {
+    return true;
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/button/Button.java

@@ -0,0 +1,211 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command.button;
+
+import java.util.function.BooleanSupplier;
+
+import edu.wpi.first.wpilibj2.command.Command;
+
+/**
+ * This class provides an easy way to link commands to OI inputs.
+ *
+ * <p>It is very easy to link a button to a command. For instance, you could link the trigger
+ * button of a joystick to a "score" command.
+ *
+ * <p>This class represents a subclass of Trigger that is specifically aimed at buttons on an
+ * operator interface as a common use case of the more generalized Trigger objects. This is a simple
+ * wrapper around Trigger with the method names renamed to fit the Button object use.
+ */
+@SuppressWarnings("PMD.TooManyMethods")
+public abstract class Button extends Trigger {
+  /**
+   * Default constructor; creates a button that is never pressed (unless {@link Button#get()} is
+   * overridden).
+   */
+  public Button() {
+  }
+
+  /**
+   * Creates a new button with the given condition determining whether it is pressed.
+   *
+   * @param isPressed returns whether or not the trigger should be active
+   */
+  public Button(BooleanSupplier isPressed) {
+    super(isPressed);
+  }
+
+  /**
+   * Starts the given command whenever the button is newly pressed.
+   *
+   * @param command       the command to start
+   * @param interruptible whether the command is interruptible
+   * @return this button, so calls can be chained
+   */
+  public Button whenPressed(final Command command, boolean interruptible) {
+    whenActive(command, interruptible);
+    return this;
+  }
+
+  /**
+   * Starts the given command whenever the button is newly pressed. The command is set to be
+   * interruptible.
+   *
+   * @param command the command to start
+   * @return this button, so calls can be chained
+   */
+  public Button whenPressed(final Command command) {
+    whenActive(command);
+    return this;
+  }
+
+  /**
+   * Runs the given runnable whenever the button is newly pressed.
+   *
+   * @param toRun the runnable to run
+   * @return this button, so calls can be chained
+   */
+  public Button whenPressed(final Runnable toRun) {
+    whenActive(toRun);
+    return this;
+  }
+
+  /**
+   * Constantly starts the given command while the button is held.
+   *
+   * {@link Command#schedule(boolean)} will be called repeatedly while the button is held, and will
+   * be canceled when the button is released.
+   *
+   * @param command       the command to start
+   * @param interruptible whether the command is interruptible
+   * @return this button, so calls can be chained
+   */
+  public Button whileHeld(final Command command, boolean interruptible) {
+    whileActiveContinuous(command, interruptible);
+    return this;
+  }
+
+  /**
+   * Constantly starts the given command while the button is held.
+   *
+   * {@link Command#schedule(boolean)} will be called repeatedly while the button is held, and will
+   * be canceled when the button is released.  The command is set to be interruptible.
+   *
+   * @param command the command to start
+   * @return this button, so calls can be chained
+   */
+  public Button whileHeld(final Command command) {
+    whileActiveContinuous(command);
+    return this;
+  }
+
+  /**
+   * Constantly runs the given runnable while the button is held.
+   *
+   * @param toRun the runnable to run
+   * @return this button, so calls can be chained
+   */
+  public Button whileHeld(final Runnable toRun) {
+    whileActiveContinuous(toRun);
+    return this;
+  }
+
+  /**
+   * Starts the given command when the button is first pressed, and cancels it when it is released,
+   * but does not start it again if it ends or is otherwise interrupted.
+   *
+   * @param command       the command to start
+   * @param interruptible whether the command is interruptible
+   * @return this button, so calls can be chained
+   */
+  public Button whenHeld(final Command command, boolean interruptible) {
+    whileActiveOnce(command, interruptible);
+    return this;
+  }
+
+  /**
+   * Starts the given command when the button is first pressed, and cancels it when it is released,
+   * but does not start it again if it ends or is otherwise interrupted.  The command is set to be
+   * interruptible.
+   *
+   * @param command the command to start
+   * @return this button, so calls can be chained
+   */
+  public Button whenHeld(final Command command) {
+    whileActiveOnce(command, true);
+    return this;
+  }
+
+
+  /**
+   * Starts the command when the button is released.
+   *
+   * @param command       the command to start
+   * @param interruptible whether the command is interruptible
+   * @return this button, so calls can be chained
+   */
+  public Button whenReleased(final Command command, boolean interruptible) {
+    whenInactive(command, interruptible);
+    return this;
+  }
+
+  /**
+   * Starts the command when the button is released.  The command is set to be interruptible.
+   *
+   * @param command the command to start
+   * @return this button, so calls can be chained
+   */
+  public Button whenReleased(final Command command) {
+    whenInactive(command);
+    return this;
+  }
+
+  /**
+   * Runs the given runnable when the button is released.
+   *
+   * @param toRun the runnable to run
+   * @return this button, so calls can be chained
+   */
+  public Button whenReleased(final Runnable toRun) {
+    whenInactive(toRun);
+    return this;
+  }
+
+  /**
+   * Toggles the command whenever the button is pressed (on then off then on).
+   *
+   * @param command       the command to start
+   * @param interruptible whether the command is interruptible
+   */
+  public Button toggleWhenPressed(final Command command, boolean interruptible) {
+    toggleWhenActive(command, interruptible);
+    return this;
+  }
+
+  /**
+   * Toggles the command whenever the button is pressed (on then off then on).  The command is set
+   * to be interruptible.
+   *
+   * @param command the command to start
+   * @return this button, so calls can be chained
+   */
+  public Button toggleWhenPressed(final Command command) {
+    toggleWhenActive(command);
+    return this;
+  }
+
+  /**
+   * Cancels the command when the button is pressed.
+   *
+   * @param command the command to start
+   * @return this button, so calls can be chained
+   */
+  public Button cancelWhenPressed(final Command command) {
+    cancelWhenActive(command);
+    return this;
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/button/InternalButton.java

@@ -0,0 +1,47 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command.button;
+
+/**
+ * This class is intended to be used within a program. The programmer can manually set its value.
+ * Also includes a setting for whether or not it should invert its value.
+ */
+public class InternalButton extends Button {
+  private boolean m_pressed;
+  private boolean m_inverted;
+
+  /**
+   * Creates an InternalButton that is not inverted.
+   */
+  public InternalButton() {
+    this(false);
+  }
+
+  /**
+   * Creates an InternalButton which is inverted depending on the input.
+   *
+   * @param inverted if false, then this button is pressed when set to true, otherwise it is pressed
+   *                 when set to false.
+   */
+  public InternalButton(boolean inverted) {
+    m_pressed = m_inverted = inverted;
+  }
+
+  public void setInverted(boolean inverted) {
+    m_inverted = inverted;
+  }
+
+  public void setPressed(boolean pressed) {
+    m_pressed = pressed;
+  }
+
+  @Override
+  public boolean get() {
+    return m_pressed ^ m_inverted;
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/button/JoystickButton.java

@@ -0,0 +1,44 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command.button;
+
+import edu.wpi.first.wpilibj.GenericHID;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * A {@link Button} that gets its state from a {@link GenericHID}.
+ */
+public class JoystickButton extends Button {
+  private final GenericHID m_joystick;
+  private final int m_buttonNumber;
+
+  /**
+   * Creates a joystick button for triggering commands.
+   *
+   * @param joystick     The GenericHID object that has the button (e.g. Joystick, KinectStick,
+   *                     etc)
+   * @param buttonNumber The button number (see {@link GenericHID#getRawButton(int) }
+   */
+  public JoystickButton(GenericHID joystick, int buttonNumber) {
+    requireNonNullParam(joystick, "joystick", "JoystickButton");
+
+    m_joystick = joystick;
+    m_buttonNumber = buttonNumber;
+  }
+
+  /**
+   * Gets the value of the joystick button.
+   *
+   * @return The value of the joystick button
+   */
+  @Override
+  public boolean get() {
+    return m_joystick.getRawButton(m_buttonNumber);
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/button/POVButton.java

@@ -0,0 +1,57 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command.button;
+
+import edu.wpi.first.wpilibj.GenericHID;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * A {@link Button} that gets its state from a POV on a {@link GenericHID}.
+ */
+public class POVButton extends Button {
+  private final GenericHID m_joystick;
+  private final int m_angle;
+  private final int m_povNumber;
+
+  /**
+   * Creates a POV button for triggering commands.
+   *
+   * @param joystick The GenericHID object that has the POV
+   * @param angle The desired angle in degrees (e.g. 90, 270)
+   * @param povNumber The POV number (see {@link GenericHID#getPOV(int)})
+   */
+  public POVButton(GenericHID joystick, int angle, int povNumber) {
+    requireNonNullParam(joystick, "joystick", "POVButton");
+
+    m_joystick = joystick;
+    m_angle = angle;
+    m_povNumber = povNumber;
+  }
+
+  /**
+   * Creates a POV button for triggering commands.
+   * By default, acts on POV 0
+   *
+   * @param joystick The GenericHID object that has the POV
+   * @param angle The desired angle (e.g. 90, 270)
+   */
+  public POVButton(GenericHID joystick, int angle) {
+    this(joystick, angle, 0);
+  }
+
+  /**
+   * Checks whether the current value of the POV is the target angle.
+   *
+   * @return Whether the value of the POV matches the target angle
+   */
+  @Override
+  public boolean get() {
+    return m_joystick.getPOV(m_povNumber) == m_angle;
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java

@@ -0,0 +1,351 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command.button;
+
+import java.util.function.BooleanSupplier;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * This class provides an easy way to link commands to inputs.
+ *
+ * <p>It is very easy to link a button to a command. For instance, you could link the trigger
+ * button of a joystick to a "score" command.
+ *
+ * <p>It is encouraged that teams write a subclass of Trigger if they want to have something
+ * unusual (for instance, if they want to react to the user holding a button while the robot is
+ * reading a certain sensor input). For this, they only have to write the {@link Trigger#get()}
+ * method to get the full functionality of the Trigger class.
+ */
+@SuppressWarnings("PMD.TooManyMethods")
+public class Trigger {
+  private final BooleanSupplier m_isActive;
+
+  /**
+   * Creates a new trigger with the given condition determining whether it is active.
+   *
+   * @param isActive returns whether or not the trigger should be active
+   */
+  public Trigger(BooleanSupplier isActive) {
+    m_isActive = isActive;
+  }
+
+  /**
+   * Creates a new trigger that is always inactive.  Useful only as a no-arg constructor for
+   * subclasses that will be overriding {@link Trigger#get()} anyway.
+   */
+  public Trigger() {
+    m_isActive = () -> false;
+  }
+
+  /**
+   * Returns whether or not the trigger is active.
+   *
+   * <p>This method will be called repeatedly a command is linked to the Trigger.
+   *
+   * @return whether or not the trigger condition is active.
+   */
+  public boolean get() {
+    return m_isActive.getAsBoolean();
+  }
+
+  /**
+   * Starts the given command whenever the trigger just becomes active.
+   *
+   * @param command       the command to start
+   * @param interruptible whether the command is interruptible
+   * @return this trigger, so calls can be chained
+   */
+  public Trigger whenActive(final Command command, boolean interruptible) {
+    requireNonNullParam(command, "command", "whenActive");
+
+    CommandScheduler.getInstance().addButton(new Runnable() {
+      private boolean m_pressedLast = get();
+
+      @Override
+      public void run() {
+        boolean pressed = get();
+
+        if (!m_pressedLast && pressed) {
+          command.schedule(interruptible);
+        }
+
+        m_pressedLast = pressed;
+      }
+    });
+
+    return this;
+  }
+
+  /**
+   * Starts the given command whenever the trigger just becomes active.  The command is set to be
+   * interruptible.
+   *
+   * @param command the command to start
+   * @return this trigger, so calls can be chained
+   */
+  public Trigger whenActive(final Command command) {
+    return whenActive(command, true);
+  }
+
+  /**
+   * Runs the given runnable whenever the trigger just becomes active.
+   *
+   * @param toRun the runnable to run
+   * @return this trigger, so calls can be chained
+   */
+  public Trigger whenActive(final Runnable toRun) {
+    return whenActive(new InstantCommand(toRun));
+  }
+
+  /**
+   * Constantly starts the given command while the button is held.
+   *
+   * {@link Command#schedule(boolean)} will be called repeatedly while the trigger is active, and
+   * will be canceled when the trigger becomes inactive.
+   *
+   * @param command       the command to start
+   * @param interruptible whether the command is interruptible
+   * @return this trigger, so calls can be chained
+   */
+  public Trigger whileActiveContinuous(final Command command, boolean interruptible) {
+    requireNonNullParam(command, "command", "whileActiveContinuous");
+
+    CommandScheduler.getInstance().addButton(new Runnable() {
+      private boolean m_pressedLast = get();
+
+      @Override
+      public void run() {
+        boolean pressed = get();
+
+        if (pressed) {
+          command.schedule(interruptible);
+        } else if (m_pressedLast) {
+          command.cancel();
+        }
+
+        m_pressedLast = pressed;
+      }
+    });
+    return this;
+  }
+
+  /**
+   * Constantly starts the given command while the button is held.
+   *
+   * {@link Command#schedule(boolean)} will be called repeatedly while the trigger is active, and
+   * will be canceled when the trigger becomes inactive.  The command is set to be interruptible.
+   *
+   * @param command the command to start
+   * @return this trigger, so calls can be chained
+   */
+  public Trigger whileActiveContinuous(final Command command) {
+    return whileActiveContinuous(command, true);
+  }
+
+  /**
+   * Constantly runs the given runnable while the button is held.
+   *
+   * @param toRun the runnable to run
+   * @return this trigger, so calls can be chained
+   */
+  public Trigger whileActiveContinuous(final Runnable toRun) {
+    return whileActiveContinuous(new InstantCommand(toRun));
+  }
+
+  /**
+   * Starts the given command when the trigger initially becomes active, and ends it when it becomes
+   * inactive, but does not re-start it in-between.
+   *
+   * @param command       the command to start
+   * @param interruptible whether the command is interruptible
+   * @return this trigger, so calls can be chained
+   */
+  public Trigger whileActiveOnce(final Command command, boolean interruptible) {
+    requireNonNullParam(command, "command", "whileActiveOnce");
+
+    CommandScheduler.getInstance().addButton(new Runnable() {
+      private boolean m_pressedLast = get();
+
+      @Override
+      public void run() {
+        boolean pressed = get();
+
+        if (!m_pressedLast && pressed) {
+          command.schedule(interruptible);
+        } else if (m_pressedLast && !pressed) {
+          command.cancel();
+        }
+
+        m_pressedLast = pressed;
+      }
+    });
+    return this;
+  }
+
+  /**
+   * Starts the given command when the trigger initially becomes active, and ends it when it becomes
+   * inactive, but does not re-start it in-between.  The command is set to be interruptible.
+   *
+   * @param command the command to start
+   * @return this trigger, so calls can be chained
+   */
+  public Trigger whileActiveOnce(final Command command) {
+    return whileActiveOnce(command, true);
+  }
+
+  /**
+   * Starts the command when the trigger becomes inactive.
+   *
+   * @param command       the command to start
+   * @param interruptible whether the command is interruptible
+   * @return this trigger, so calls can be chained
+   */
+  public Trigger whenInactive(final Command command, boolean interruptible) {
+    requireNonNullParam(command, "command", "whenInactive");
+
+    CommandScheduler.getInstance().addButton(new Runnable() {
+      private boolean m_pressedLast = get();
+
+      @Override
+      public void run() {
+        boolean pressed = get();
+
+        if (m_pressedLast && !pressed) {
+          command.schedule(interruptible);
+        }
+
+        m_pressedLast = pressed;
+      }
+    });
+    return this;
+  }
+
+  /**
+   * Starts the command when the trigger becomes inactive.  The command is set to be interruptible.
+   *
+   * @param command the command to start
+   * @return this trigger, so calls can be chained
+   */
+  public Trigger whenInactive(final Command command) {
+    return whenInactive(command, true);
+  }
+
+  /**
+   * Runs the given runnable when the trigger becomes inactive.
+   *
+   * @param toRun the runnable to run
+   * @return this trigger, so calls can be chained
+   */
+  public Trigger whenInactive(final Runnable toRun) {
+    return whenInactive(new InstantCommand(toRun));
+  }
+
+  /**
+   * Toggles a command when the trigger becomes active.
+   *
+   * @param command       the command to toggle
+   * @param interruptible whether the command is interruptible
+   * @return this trigger, so calls can be chained
+   */
+  public Trigger toggleWhenActive(final Command command, boolean interruptible) {
+    requireNonNullParam(command, "command", "toggleWhenActive");
+
+    CommandScheduler.getInstance().addButton(new Runnable() {
+      private boolean m_pressedLast = get();
+
+      @Override
+      public void run() {
+        boolean pressed = get();
+
+        if (!m_pressedLast && pressed) {
+          if (command.isScheduled()) {
+            command.cancel();
+          } else {
+            command.schedule(interruptible);
+          }
+        }
+
+        m_pressedLast = pressed;
+      }
+    });
+    return this;
+  }
+
+  /**
+   * Toggles a command when the trigger becomes active.  The command is set to be interruptible.
+   *
+   * @param command the command to toggle
+   * @return this trigger, so calls can be chained
+   */
+  public Trigger toggleWhenActive(final Command command) {
+    return toggleWhenActive(command, true);
+  }
+
+  /**
+   * Cancels a command when the trigger becomes active.
+   *
+   * @param command the command to cancel
+   * @return this trigger, so calls can be chained
+   */
+  public Trigger cancelWhenActive(final Command command) {
+    requireNonNullParam(command, "command", "cancelWhenActive");
+
+    CommandScheduler.getInstance().addButton(new Runnable() {
+      private boolean m_pressedLast = get();
+
+      @Override
+      public void run() {
+        boolean pressed = get();
+
+        if (!m_pressedLast && pressed) {
+          command.cancel();
+        }
+
+        m_pressedLast = pressed;
+      }
+    });
+    return this;
+  }
+
+  /**
+   * Composes this trigger with another trigger, returning a new trigger that is active when both
+   * triggers are active.
+   *
+   * @param trigger the trigger to compose with
+   * @return the trigger that is active when both triggers are active
+   */
+  public Trigger and(Trigger trigger) {
+    return new Trigger(() -> get() && trigger.get());
+  }
+
+  /**
+   * Composes this trigger with another trigger, returning a new trigger that is active when either
+   * trigger is active.
+   *
+   * @param trigger the trigger to compose with
+   * @return the trigger that is active when either trigger is active
+   */
+  public Trigger or(Trigger trigger) {
+    return new Trigger(() -> get() || trigger.get());
+  }
+
+  /**
+   * Creates a new trigger that is active when this trigger is inactive, i.e. that acts as the
+   * negation of this trigger.
+   *
+   * @return the negated trigger
+   */
+  public Trigger negate() {
+    return new Trigger(() -> !get());
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/ButtonTest.java

@@ -0,0 +1,179 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj2.command.button.InternalButton;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.mockito.Mockito.never;
+import static org.mockito.Mockito.times;
+import static org.mockito.Mockito.verify;
+import static org.mockito.Mockito.when;
+
+
+class ButtonTest extends CommandTestBase {
+  @Test
+  void whenPressedTest() {
+    CommandScheduler scheduler = CommandScheduler.getInstance();
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+
+    InternalButton button = new InternalButton();
+    button.setPressed(false);
+    button.whenPressed(command1);
+    scheduler.run();
+    verify(command1, never()).schedule(true);
+    button.setPressed(true);
+    scheduler.run();
+    scheduler.run();
+    verify(command1).schedule(true);
+  }
+
+  @Test
+  void whenReleasedTest() {
+    CommandScheduler scheduler = CommandScheduler.getInstance();
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+
+    InternalButton button = new InternalButton();
+    button.setPressed(true);
+    button.whenReleased(command1);
+    scheduler.run();
+    verify(command1, never()).schedule(true);
+    button.setPressed(false);
+    scheduler.run();
+    scheduler.run();
+    verify(command1).schedule(true);
+  }
+
+  @Test
+  void whileHeldTest() {
+    CommandScheduler scheduler = CommandScheduler.getInstance();
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+
+    InternalButton button = new InternalButton();
+    button.setPressed(false);
+    button.whileHeld(command1);
+    scheduler.run();
+    verify(command1, never()).schedule(true);
+    button.setPressed(true);
+    scheduler.run();
+    scheduler.run();
+    verify(command1, times(2)).schedule(true);
+    button.setPressed(false);
+    scheduler.run();
+    verify(command1).cancel();
+  }
+
+  @Test
+  void whenHeldTest() {
+    CommandScheduler scheduler = CommandScheduler.getInstance();
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+
+    InternalButton button = new InternalButton();
+    button.setPressed(false);
+    button.whenHeld(command1);
+    scheduler.run();
+    verify(command1, never()).schedule(true);
+    button.setPressed(true);
+    scheduler.run();
+    scheduler.run();
+    verify(command1).schedule(true);
+    button.setPressed(false);
+    scheduler.run();
+    verify(command1).cancel();
+  }
+
+  @Test
+  void toggleWhenPressedTest() {
+    CommandScheduler scheduler = CommandScheduler.getInstance();
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+
+    InternalButton button = new InternalButton();
+    button.setPressed(false);
+    button.toggleWhenPressed(command1);
+    scheduler.run();
+    verify(command1, never()).schedule(true);
+    button.setPressed(true);
+    scheduler.run();
+    when(command1.isScheduled()).thenReturn(true);
+    scheduler.run();
+    verify(command1).schedule(true);
+    button.setPressed(false);
+    scheduler.run();
+    verify(command1, never()).cancel();
+    button.setPressed(true);
+    scheduler.run();
+    verify(command1).cancel();
+  }
+
+  @Test
+  void cancelWhenPressedTest() {
+    CommandScheduler scheduler = CommandScheduler.getInstance();
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+
+    InternalButton button = new InternalButton();
+    button.setPressed(false);
+    button.cancelWhenPressed(command1);
+    scheduler.run();
+    verify(command1, never()).cancel();
+    button.setPressed(true);
+    scheduler.run();
+    scheduler.run();
+    verify(command1).cancel();
+  }
+
+  @Test
+  void runnableBindingTest() {
+
+    InternalButton buttonWhenPressed = new InternalButton();
+    InternalButton buttonWhileHeld = new InternalButton();
+    InternalButton buttonWhenReleased = new InternalButton();
+
+    buttonWhenPressed.setPressed(false);
+    buttonWhileHeld.setPressed(true);
+    buttonWhenReleased.setPressed(true);
+
+    Counter counter = new Counter();
+
+    buttonWhenPressed.whenPressed(counter::increment);
+    buttonWhileHeld.whileHeld(counter::increment);
+    buttonWhenReleased.whenReleased(counter::increment);
+
+    CommandScheduler scheduler = CommandScheduler.getInstance();
+
+    scheduler.run();
+    buttonWhenPressed.setPressed(true);
+    buttonWhenReleased.setPressed(false);
+    scheduler.run();
+
+    assertEquals(counter.m_counter, 4);
+  }
+
+  @Test
+  void buttonCompositionTest() {
+    InternalButton button1 = new InternalButton();
+    InternalButton button2 = new InternalButton();
+
+    button1.setPressed(true);
+    button2.setPressed(false);
+
+    assertFalse(button1.and(button2).get());
+    assertTrue(button1.or(button2).get());
+    assertFalse(button1.negate().get());
+    assertTrue(button1.and(button2.negate()).get());
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/CommandDecoratorTest.java

@@ -0,0 +1,182 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.Timer;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class CommandDecoratorTest extends CommandTestBase {
+  @Test
+  void withTimeoutTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    Command command1 = new WaitCommand(10);
+
+    Command timeout = command1.withTimeout(2);
+
+    scheduler.schedule(timeout);
+    scheduler.run();
+
+    assertFalse(scheduler.isScheduled(command1));
+    assertTrue(scheduler.isScheduled(timeout));
+
+    Timer.delay(3);
+    scheduler.run();
+
+    assertFalse(scheduler.isScheduled(timeout));
+  }
+
+  @Test
+  void interruptOnTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    ConditionHolder condition = new ConditionHolder();
+
+    Command command = new WaitCommand(10).interruptOn(condition::getCondition);
+
+    scheduler.schedule(command);
+    scheduler.run();
+    assertTrue(scheduler.isScheduled(command));
+    condition.setCondition(true);
+    scheduler.run();
+    assertFalse(scheduler.isScheduled(command));
+  }
+
+  @Test
+  void beforeStartingTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    ConditionHolder condition = new ConditionHolder();
+    condition.setCondition(false);
+
+    Command command = new InstantCommand();
+
+    scheduler.schedule(command.beforeStarting(() -> condition.setCondition(true)));
+
+    assertTrue(condition.getCondition());
+  }
+
+  @Test
+  void whenFinishedTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    ConditionHolder condition = new ConditionHolder();
+    condition.setCondition(false);
+
+    Command command = new InstantCommand();
+
+    scheduler.schedule(command.whenFinished(() -> condition.setCondition(true)));
+
+    assertFalse(condition.getCondition());
+
+    scheduler.run();
+
+    assertTrue(condition.getCondition());
+  }
+
+  @Test
+  void andThenTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    ConditionHolder condition = new ConditionHolder();
+    condition.setCondition(false);
+
+    Command command1 = new InstantCommand();
+    Command command2 = new InstantCommand(() -> condition.setCondition(true));
+
+    scheduler.schedule(command1.andThen(command2));
+
+    assertFalse(condition.getCondition());
+
+    scheduler.run();
+
+    assertTrue(condition.getCondition());
+  }
+
+  @Test
+  void deadlineWithTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    ConditionHolder condition = new ConditionHolder();
+    condition.setCondition(false);
+
+    Command dictator = new WaitUntilCommand(condition::getCondition);
+    Command endsBefore = new InstantCommand();
+    Command endsAfter = new WaitUntilCommand(() -> false);
+
+    Command group = dictator.deadlineWith(endsBefore, endsAfter);
+
+    scheduler.schedule(group);
+    scheduler.run();
+
+    assertTrue(scheduler.isScheduled(group));
+
+    condition.setCondition(true);
+    scheduler.run();
+
+    assertFalse(scheduler.isScheduled(group));
+  }
+
+  @Test
+  void alongWithTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    ConditionHolder condition = new ConditionHolder();
+    condition.setCondition(false);
+
+    Command command1 = new WaitUntilCommand(condition::getCondition);
+    Command command2 = new InstantCommand();
+
+    Command group = command1.alongWith(command2);
+
+    scheduler.schedule(group);
+    scheduler.run();
+
+    assertTrue(scheduler.isScheduled(group));
+
+    condition.setCondition(true);
+    scheduler.run();
+
+    assertFalse(scheduler.isScheduled(group));
+  }
+
+  @Test
+  void raceWithTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    Command command1 = new WaitUntilCommand(() -> false);
+    Command command2 = new InstantCommand();
+
+    Command group = command1.raceWith(command2);
+
+    scheduler.schedule(group);
+    scheduler.run();
+
+    assertFalse(scheduler.isScheduled(group));
+  }
+
+  @Test
+  void perpetuallyTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    Command command = new InstantCommand();
+
+    Command perpetual = command.perpetually();
+
+    scheduler.schedule(perpetual);
+    scheduler.run();
+    scheduler.run();
+    scheduler.run();
+
+    assertTrue(scheduler.isScheduled(perpetual));
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/CommandGroupErrorTest.java

@@ -0,0 +1,55 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+
+class CommandGroupErrorTest extends CommandTestBase {
+  @Test
+  void commandInMultipleGroupsTest() {
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true);
+    Command command2 = command2Holder.getMock();
+
+    @SuppressWarnings("PMD.UnusedLocalVariable")
+    Command group = new ParallelCommandGroup(command1, command2);
+    assertThrows(IllegalArgumentException.class,
+        () -> new ParallelCommandGroup(command1, command2));
+  }
+
+  @Test
+  void commandInGroupExternallyScheduledTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true);
+    Command command2 = command2Holder.getMock();
+
+    @SuppressWarnings("PMD.UnusedLocalVariable")
+    Command group = new ParallelCommandGroup(command1, command2);
+
+    assertThrows(IllegalArgumentException.class,
+        () -> scheduler.schedule(command1));
+  }
+
+  @Test
+  void redecoratedCommandErrorTest() {
+    Command command = new InstantCommand();
+
+    assertDoesNotThrow(() -> command.withTimeout(10).interruptOn(() -> false));
+    assertThrows(IllegalArgumentException.class, () -> command.withTimeout(10));
+    CommandGroupBase.clearGroupedCommand(command);
+    assertDoesNotThrow(() -> command.withTimeout(10));
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/CommandRequirementsTest.java

@@ -0,0 +1,79 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.mockito.Mockito.verify;
+
+@SuppressWarnings("PMD.TooManyMethods")
+class CommandRequirementsTest extends CommandTestBase {
+  @Test
+  void requirementInterruptTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    Subsystem requirement = new TestSubsystem();
+
+    MockCommandHolder interruptedHolder = new MockCommandHolder(true, requirement);
+    Command interrupted = interruptedHolder.getMock();
+    MockCommandHolder interrupterHolder = new MockCommandHolder(true, requirement);
+    Command interrupter = interrupterHolder.getMock();
+
+    scheduler.schedule(interrupted);
+    scheduler.run();
+    scheduler.schedule(interrupter);
+    scheduler.run();
+
+    verify(interrupted).initialize();
+    verify(interrupted).execute();
+    verify(interrupted).end(true);
+
+    verify(interrupter).initialize();
+    verify(interrupter).execute();
+
+    assertFalse(scheduler.isScheduled(interrupted));
+    assertTrue(scheduler.isScheduled(interrupter));
+  }
+
+  @Test
+  void requirementUninterruptibleTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    Subsystem requirement = new TestSubsystem();
+
+    MockCommandHolder interruptedHolder = new MockCommandHolder(true, requirement);
+    Command notInterrupted = interruptedHolder.getMock();
+    MockCommandHolder interrupterHolder = new MockCommandHolder(true, requirement);
+    Command interrupter = interrupterHolder.getMock();
+
+    scheduler.schedule(false, notInterrupted);
+    scheduler.schedule(interrupter);
+
+    assertTrue(scheduler.isScheduled(notInterrupted));
+    assertFalse(scheduler.isScheduled(interrupter));
+  }
+
+  @Test
+  void defaultCommandRequirementErrorTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    Subsystem system = new TestSubsystem();
+
+    Command missingRequirement = new WaitUntilCommand(() -> false);
+    Command ends = new InstantCommand(() -> {
+    }, system);
+
+    assertThrows(IllegalArgumentException.class,
+        () -> scheduler.setDefaultCommand(system, missingRequirement));
+    assertThrows(IllegalArgumentException.class,
+        () -> scheduler.setDefaultCommand(system, ends));
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/CommandScheduleTest.java

@@ -0,0 +1,122 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.mockito.Mockito.never;
+import static org.mockito.Mockito.times;
+import static org.mockito.Mockito.verify;
+
+class CommandScheduleTest extends CommandTestBase {
+  @Test
+  void instantScheduleTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder holder = new MockCommandHolder(true);
+    holder.setFinished(true);
+    Command mockCommand = holder.getMock();
+
+    scheduler.schedule(mockCommand);
+    assertTrue(scheduler.isScheduled(mockCommand));
+    verify(mockCommand).initialize();
+
+    scheduler.run();
+
+    verify(mockCommand).execute();
+    verify(mockCommand).end(false);
+
+    assertFalse(scheduler.isScheduled(mockCommand));
+  }
+
+  @Test
+  void singleIterationScheduleTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder holder = new MockCommandHolder(true);
+    Command mockCommand = holder.getMock();
+
+    scheduler.schedule(mockCommand);
+
+    assertTrue(scheduler.isScheduled(mockCommand));
+
+    scheduler.run();
+    holder.setFinished(true);
+    scheduler.run();
+
+    verify(mockCommand).initialize();
+    verify(mockCommand, times(2)).execute();
+    verify(mockCommand).end(false);
+
+    assertFalse(scheduler.isScheduled(mockCommand));
+  }
+
+  @Test
+  void multiScheduleTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true);
+    Command command2 = command2Holder.getMock();
+    MockCommandHolder command3Holder = new MockCommandHolder(true);
+    Command command3 = command3Holder.getMock();
+
+    scheduler.schedule(true, command1, command2, command3);
+    assertTrue(scheduler.isScheduled(command1, command2, command3));
+    scheduler.run();
+    assertTrue(scheduler.isScheduled(command1, command2, command3));
+
+    command1Holder.setFinished(true);
+    scheduler.run();
+    assertTrue(scheduler.isScheduled(command2, command3));
+    assertFalse(scheduler.isScheduled(command1));
+
+    command2Holder.setFinished(true);
+    scheduler.run();
+    assertTrue(scheduler.isScheduled(command3));
+    assertFalse(scheduler.isScheduled(command1, command2));
+
+    command3Holder.setFinished(true);
+    scheduler.run();
+    assertFalse(scheduler.isScheduled(command1, command2, command3));
+  }
+
+  @Test
+  void schedulerCancelTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder holder = new MockCommandHolder(true);
+    Command mockCommand = holder.getMock();
+
+    scheduler.schedule(mockCommand);
+
+    scheduler.run();
+    scheduler.cancel(mockCommand);
+    scheduler.run();
+
+    verify(mockCommand).execute();
+    verify(mockCommand).end(true);
+    verify(mockCommand, never()).end(false);
+
+    assertFalse(scheduler.isScheduled(mockCommand));
+  }
+
+  @Test
+  void notScheduledCancelTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder holder = new MockCommandHolder(true);
+    Command mockCommand = holder.getMock();
+
+    assertDoesNotThrow(() -> scheduler.cancel(mockCommand));
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/CommandTestBase.java

@@ -0,0 +1,92 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.Set;
+
+import org.junit.jupiter.api.BeforeEach;
+
+import edu.wpi.first.hal.sim.DriverStationSim;
+import edu.wpi.first.wpilibj.DriverStation;
+
+import static org.mockito.Mockito.mock;
+import static org.mockito.Mockito.when;
+
+/**
+ * Basic setup for all {@link Command tests}."
+ */
+@SuppressWarnings("PMD.AbstractClassWithoutAbstractMethod")
+abstract class CommandTestBase {
+  @BeforeEach
+  void commandSetup() {
+    CommandScheduler.getInstance().cancelAll();
+    CommandScheduler.getInstance().enable();
+    CommandScheduler.getInstance().clearButtons();
+    CommandGroupBase.clearGroupedCommands();
+
+    setDSEnabled(true);
+  }
+
+  void setDSEnabled(boolean enabled) {
+    DriverStationSim sim = new DriverStationSim();
+    sim.setDsAttached(true);
+
+    sim.setEnabled(enabled);
+    sim.notifyNewData();
+    DriverStation.getInstance().isNewControlData();
+    while (DriverStation.getInstance().isEnabled() != enabled) {
+      try {
+        Thread.sleep(1);
+      } catch (InterruptedException exception) {
+        exception.printStackTrace();
+      }
+    }
+  }
+
+  class TestSubsystem extends SubsystemBase {
+  }
+
+  protected class MockCommandHolder {
+    private final Command m_mockCommand = mock(Command.class);
+
+    MockCommandHolder(boolean runWhenDisabled, Subsystem... requirements) {
+      when(m_mockCommand.getRequirements()).thenReturn(Set.of(requirements));
+      when(m_mockCommand.isFinished()).thenReturn(false);
+      when(m_mockCommand.runsWhenDisabled()).thenReturn(runWhenDisabled);
+    }
+
+    Command getMock() {
+      return m_mockCommand;
+    }
+
+    void setFinished(boolean finished) {
+      when(m_mockCommand.isFinished()).thenReturn(finished);
+    }
+
+  }
+
+  protected class Counter {
+    int m_counter;
+
+    void increment() {
+      m_counter++;
+    }
+  }
+
+  protected class ConditionHolder {
+    private boolean m_condition;
+
+    void setCondition(boolean condition) {
+      m_condition = condition;
+    }
+
+    boolean getCondition() {
+      return m_condition;
+    }
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/ConditionalCommandTest.java

@@ -0,0 +1,65 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.mockito.Mockito.never;
+import static org.mockito.Mockito.verify;
+
+class ConditionalCommandTest extends CommandTestBase {
+  @Test
+  void conditionalCommandTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+    command1Holder.setFinished(true);
+    MockCommandHolder command2Holder = new MockCommandHolder(true);
+    Command command2 = command2Holder.getMock();
+
+    ConditionalCommand conditionalCommand = new ConditionalCommand(command1, command2, () -> true);
+
+    scheduler.schedule(conditionalCommand);
+    scheduler.run();
+
+    verify(command1).initialize();
+    verify(command1).execute();
+    verify(command1).end(false);
+
+    verify(command2, never()).initialize();
+    verify(command2, never()).execute();
+    verify(command2, never()).end(false);
+  }
+
+  @Test
+  void conditionalCommandRequirementTest() {
+    Subsystem system1 = new TestSubsystem();
+    Subsystem system2 = new TestSubsystem();
+    Subsystem system3 = new TestSubsystem();
+
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true, system3);
+    Command command2 = command2Holder.getMock();
+
+    ConditionalCommand conditionalCommand = new ConditionalCommand(command1, command2, () -> true);
+
+    scheduler.schedule(conditionalCommand);
+    scheduler.schedule(new InstantCommand(() -> {
+    }, system3));
+
+    assertFalse(scheduler.isScheduled(conditionalCommand));
+
+    verify(command1).end(true);
+    verify(command2, never()).end(true);
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/DefaultCommandTest.java

@@ -0,0 +1,83 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.mockito.Mockito.verify;
+
+class DefaultCommandTest extends CommandTestBase {
+  @Test
+  void defaultCommandScheduleTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    TestSubsystem hasDefaultCommand = new TestSubsystem();
+
+    MockCommandHolder defaultHolder = new MockCommandHolder(true, hasDefaultCommand);
+    Command defaultCommand = defaultHolder.getMock();
+
+    scheduler.setDefaultCommand(hasDefaultCommand, defaultCommand);
+    scheduler.run();
+
+    assertTrue(scheduler.isScheduled(defaultCommand));
+  }
+
+  @Test
+  void defaultCommandInterruptResumeTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    TestSubsystem hasDefaultCommand = new TestSubsystem();
+
+    MockCommandHolder defaultHolder = new MockCommandHolder(true, hasDefaultCommand);
+    Command defaultCommand = defaultHolder.getMock();
+    MockCommandHolder interrupterHolder = new MockCommandHolder(true, hasDefaultCommand);
+    Command interrupter = interrupterHolder.getMock();
+
+    scheduler.setDefaultCommand(hasDefaultCommand, defaultCommand);
+    scheduler.run();
+    scheduler.schedule(interrupter);
+
+    assertFalse(scheduler.isScheduled(defaultCommand));
+    assertTrue(scheduler.isScheduled(interrupter));
+
+    scheduler.cancel(interrupter);
+    scheduler.run();
+
+    assertTrue(scheduler.isScheduled(defaultCommand));
+    assertFalse(scheduler.isScheduled(interrupter));
+  }
+
+  @Test
+  void defaultCommandDisableResumeTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    TestSubsystem hasDefaultCommand = new TestSubsystem();
+
+    MockCommandHolder defaultHolder = new MockCommandHolder(false, hasDefaultCommand);
+    Command defaultCommand = defaultHolder.getMock();
+
+    scheduler.setDefaultCommand(hasDefaultCommand, defaultCommand);
+    scheduler.run();
+
+    assertTrue(scheduler.isScheduled(defaultCommand));
+
+    setDSEnabled(false);
+    scheduler.run();
+
+    assertFalse(scheduler.isScheduled(defaultCommand));
+
+    setDSEnabled(true);
+    scheduler.run();
+
+    assertTrue(scheduler.isScheduled(defaultCommand));
+
+    verify(defaultCommand).end(true);
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/FunctionalCommandTest.java

@@ -0,0 +1,43 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class FunctionalCommandTest extends CommandTestBase {
+  @Test
+  void functionalCommandScheduleTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    ConditionHolder cond1 = new ConditionHolder();
+    ConditionHolder cond2 = new ConditionHolder();
+    ConditionHolder cond3 = new ConditionHolder();
+    ConditionHolder cond4 = new ConditionHolder();
+
+    FunctionalCommand command =
+        new FunctionalCommand(() -> cond1.setCondition(true), () -> cond2.setCondition(true),
+            interrupted -> cond3.setCondition(true), cond4::getCondition);
+
+    scheduler.schedule(command);
+    scheduler.run();
+
+    assertTrue(scheduler.isScheduled(command));
+
+    cond4.setCondition(true);
+
+    scheduler.run();
+
+    assertFalse(scheduler.isScheduled(command));
+    assertTrue(cond1.getCondition());
+    assertTrue(cond2.getCondition());
+    assertTrue(cond3.getCondition());
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/InstantCommandTest.java

@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class InstantCommandTest extends CommandTestBase {
+  @Test
+  void instantCommandScheduleTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    ConditionHolder cond = new ConditionHolder();
+
+    InstantCommand command = new InstantCommand(() -> cond.setCondition(true));
+
+    scheduler.schedule(command);
+    scheduler.run();
+
+    assertTrue(cond.getCondition());
+    assertFalse(scheduler.isScheduled(command));
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/NotifierCommandTest.java

@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.api.condition.DisabledOnOs;
+import org.junit.jupiter.api.condition.OS;
+
+import edu.wpi.first.wpilibj.Timer;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+@DisabledOnOs(OS.MAC)
+class NotifierCommandTest extends CommandTestBase {
+  @Test
+  void notifierCommandScheduleTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    Counter counter = new Counter();
+
+    NotifierCommand command = new NotifierCommand(counter::increment, .01);
+
+    scheduler.schedule(command);
+    Timer.delay(.25);
+    scheduler.cancel(command);
+
+    assertEquals(.25, 0.01 * counter.m_counter, .025);
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroupTest.java

@@ -0,0 +1,131 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.mockito.Mockito.never;
+import static org.mockito.Mockito.times;
+import static org.mockito.Mockito.verify;
+
+class ParallelCommandGroupTest extends CommandTestBase {
+  @Test
+  void parallelGroupScheduleTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true);
+    Command command2 = command2Holder.getMock();
+
+    Command group = new ParallelCommandGroup(command1, command2);
+
+    scheduler.schedule(group);
+
+    verify(command1).initialize();
+    verify(command2).initialize();
+
+    command1Holder.setFinished(true);
+    scheduler.run();
+    command2Holder.setFinished(true);
+    scheduler.run();
+
+    verify(command1).execute();
+    verify(command1).end(false);
+    verify(command2, times(2)).execute();
+    verify(command2).end(false);
+
+    assertFalse(scheduler.isScheduled(group));
+  }
+
+  @Test
+  void parallelGroupInterruptTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true);
+    Command command2 = command2Holder.getMock();
+
+    Command group = new ParallelCommandGroup(command1, command2);
+
+    scheduler.schedule(group);
+
+    command1Holder.setFinished(true);
+    scheduler.run();
+    scheduler.run();
+    scheduler.cancel(group);
+
+    verify(command1).execute();
+    verify(command1).end(false);
+    verify(command1, never()).end(true);
+
+    verify(command2, times(2)).execute();
+    verify(command2, never()).end(false);
+    verify(command2).end(true);
+
+    assertFalse(scheduler.isScheduled(group));
+  }
+
+  @Test
+  void notScheduledCancelTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true);
+    Command command2 = command2Holder.getMock();
+
+    Command group = new ParallelCommandGroup(command1, command2);
+
+    assertDoesNotThrow(() -> scheduler.cancel(group));
+  }
+
+  @Test
+  void parallelGroupRequirementTest() {
+    Subsystem system1 = new TestSubsystem();
+    Subsystem system2 = new TestSubsystem();
+    Subsystem system3 = new TestSubsystem();
+    Subsystem system4 = new TestSubsystem();
+
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true, system3);
+    Command command2 = command2Holder.getMock();
+    MockCommandHolder command3Holder = new MockCommandHolder(true, system3, system4);
+    Command command3 = command3Holder.getMock();
+
+    Command group = new ParallelCommandGroup(command1, command2);
+
+    scheduler.schedule(group);
+    scheduler.schedule(command3);
+
+    assertFalse(scheduler.isScheduled(group));
+    assertTrue(scheduler.isScheduled(command3));
+  }
+
+  @Test
+  void parallelGroupRequirementErrorTest() {
+    Subsystem system1 = new TestSubsystem();
+    Subsystem system2 = new TestSubsystem();
+    Subsystem system3 = new TestSubsystem();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true, system2, system3);
+    Command command2 = command2Holder.getMock();
+
+    assertThrows(IllegalArgumentException.class,
+        () -> new ParallelCommandGroup(command1, command2));
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroupTest.java

@@ -0,0 +1,129 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.mockito.Mockito.never;
+import static org.mockito.Mockito.verify;
+import static org.mockito.internal.verification.VerificationModeFactory.times;
+
+class ParallelDeadlineGroupTest extends CommandTestBase {
+  @Test
+  void parallelDeadlineScheduleTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true);
+    Command command2 = command2Holder.getMock();
+    command2Holder.setFinished(true);
+    MockCommandHolder command3Holder = new MockCommandHolder(true);
+    Command command3 = command3Holder.getMock();
+
+    Command group = new ParallelDeadlineGroup(command1, command2, command3);
+
+    scheduler.schedule(group);
+    scheduler.run();
+
+    assertTrue(scheduler.isScheduled(group));
+
+    command1Holder.setFinished(true);
+    scheduler.run();
+
+    assertFalse(scheduler.isScheduled(group));
+
+    verify(command2).initialize();
+    verify(command2).execute();
+    verify(command2).end(false);
+    verify(command2, never()).end(true);
+
+    verify(command1).initialize();
+    verify(command1, times(2)).execute();
+    verify(command1).end(false);
+    verify(command1, never()).end(true);
+
+    verify(command3).initialize();
+    verify(command3, times(2)).execute();
+    verify(command3, never()).end(false);
+    verify(command3).end(true);
+  }
+
+  @Test
+  void parallelDeadlineInterruptTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true);
+    Command command2 = command2Holder.getMock();
+    command2Holder.setFinished(true);
+
+    Command group = new ParallelDeadlineGroup(command1, command2);
+
+    scheduler.schedule(group);
+
+    scheduler.run();
+    scheduler.run();
+    scheduler.cancel(group);
+
+    verify(command1, times(2)).execute();
+    verify(command1, never()).end(false);
+    verify(command1).end(true);
+
+    verify(command2).execute();
+    verify(command2).end(false);
+    verify(command2, never()).end(true);
+
+    assertFalse(scheduler.isScheduled(group));
+  }
+
+
+  @Test
+  void parallelDeadlineRequirementTest() {
+    Subsystem system1 = new TestSubsystem();
+    Subsystem system2 = new TestSubsystem();
+    Subsystem system3 = new TestSubsystem();
+    Subsystem system4 = new TestSubsystem();
+
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true, system3);
+    Command command2 = command2Holder.getMock();
+    MockCommandHolder command3Holder = new MockCommandHolder(true, system3, system4);
+    Command command3 = command3Holder.getMock();
+
+    Command group = new ParallelDeadlineGroup(command1, command2);
+
+    scheduler.schedule(group);
+    scheduler.schedule(command3);
+
+    assertFalse(scheduler.isScheduled(group));
+    assertTrue(scheduler.isScheduled(command3));
+  }
+
+  @Test
+  void parallelDeadlineRequirementErrorTest() {
+    Subsystem system1 = new TestSubsystem();
+    Subsystem system2 = new TestSubsystem();
+    Subsystem system3 = new TestSubsystem();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true, system2, system3);
+    Command command2 = command2Holder.getMock();
+
+    assertThrows(IllegalArgumentException.class,
+        () -> new ParallelDeadlineGroup(command1, command2));
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroupTest.java

@@ -0,0 +1,132 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.mockito.Mockito.never;
+import static org.mockito.Mockito.times;
+import static org.mockito.Mockito.verify;
+
+class ParallelRaceGroupTest extends CommandTestBase {
+  @Test
+  void parallelRaceScheduleTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true);
+    Command command2 = command2Holder.getMock();
+
+    Command group = new ParallelRaceGroup(command1, command2);
+
+    scheduler.schedule(group);
+
+    verify(command1).initialize();
+    verify(command2).initialize();
+
+    command1Holder.setFinished(true);
+    scheduler.run();
+    command2Holder.setFinished(true);
+    scheduler.run();
+
+    verify(command1).execute();
+    verify(command1).end(false);
+    verify(command2).execute();
+    verify(command2).end(true);
+    verify(command2, never()).end(false);
+
+    assertFalse(scheduler.isScheduled(group));
+  }
+
+  @Test
+  void parallelRaceInterruptTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true);
+    Command command2 = command2Holder.getMock();
+
+    Command group = new ParallelRaceGroup(command1, command2);
+
+    scheduler.schedule(group);
+
+    scheduler.run();
+    scheduler.run();
+    scheduler.cancel(group);
+
+    verify(command1, times(2)).execute();
+    verify(command1, never()).end(false);
+    verify(command1).end(true);
+
+    verify(command2, times(2)).execute();
+    verify(command2, never()).end(false);
+    verify(command2).end(true);
+
+    assertFalse(scheduler.isScheduled(group));
+  }
+
+  @Test
+  void notScheduledCancelTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true);
+    Command command2 = command2Holder.getMock();
+
+    Command group = new ParallelRaceGroup(command1, command2);
+
+    assertDoesNotThrow(() -> scheduler.cancel(group));
+  }
+
+
+  @Test
+  void parallelRaceRequirementTest() {
+    Subsystem system1 = new TestSubsystem();
+    Subsystem system2 = new TestSubsystem();
+    Subsystem system3 = new TestSubsystem();
+    Subsystem system4 = new TestSubsystem();
+
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true, system3);
+    Command command2 = command2Holder.getMock();
+    MockCommandHolder command3Holder = new MockCommandHolder(true, system3, system4);
+    Command command3 = command3Holder.getMock();
+
+    Command group = new ParallelRaceGroup(command1, command2);
+
+    scheduler.schedule(group);
+    scheduler.schedule(command3);
+
+    assertFalse(scheduler.isScheduled(group));
+    assertTrue(scheduler.isScheduled(command3));
+  }
+
+  @Test
+  void parallelRaceRequirementErrorTest() {
+    Subsystem system1 = new TestSubsystem();
+    Subsystem system2 = new TestSubsystem();
+    Subsystem system3 = new TestSubsystem();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true, system2, system3);
+    Command command2 = command2Holder.getMock();
+
+    assertThrows(IllegalArgumentException.class, () -> new ParallelRaceGroup(command1, command2));
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/PerpetualCommandTest.java

@@ -0,0 +1,26 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class PerpetualCommandTest extends CommandTestBase {
+  @Test
+  void perpetualCommandScheduleTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    PerpetualCommand command = new PerpetualCommand(new InstantCommand());
+
+    scheduler.schedule(command);
+    scheduler.run();
+
+    assertTrue(scheduler.isScheduled(command));
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/PrintCommandTest.java

@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.io.ByteArrayOutputStream;
+import java.io.PrintStream;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+
+class PrintCommandTest extends CommandTestBase {
+  @Test
+  void printCommandScheduleTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    final PrintStream originalOut = System.out;
+    ByteArrayOutputStream testOut = new ByteArrayOutputStream();
+    System.setOut(new PrintStream(testOut));
+
+    PrintCommand command = new PrintCommand("Test!");
+
+    scheduler.schedule(command);
+    scheduler.run();
+
+    assertFalse(scheduler.isScheduled(command));
+    assertEquals(testOut.toString(), "Test!" + System.lineSeparator());
+
+    System.setOut(originalOut);
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/ProxyScheduleCommandTest.java

@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.mockito.Mockito.verify;
+
+class ProxyScheduleCommandTest extends CommandTestBase {
+  @Test
+  void proxyScheduleCommandScheduleTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+
+    ProxyScheduleCommand scheduleCommand = new ProxyScheduleCommand(command1);
+
+    scheduler.schedule(scheduleCommand);
+
+    verify(command1).schedule();
+  }
+
+  @Test
+  void proxyScheduleCommandEndTest() {
+    CommandScheduler scheduler = CommandScheduler.getInstance();
+
+    ConditionHolder cond = new ConditionHolder();
+
+    WaitUntilCommand command = new WaitUntilCommand(cond::getCondition);
+
+    ProxyScheduleCommand scheduleCommand = new ProxyScheduleCommand(command);
+
+    scheduler.schedule(scheduleCommand);
+
+    scheduler.run();
+    assertTrue(scheduler.isScheduled(scheduleCommand));
+
+    cond.setCondition(true);
+    scheduler.run();
+    scheduler.run();
+    assertFalse(scheduler.isScheduled(scheduleCommand));
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/RobotDisabledCommandTest.java

@@ -0,0 +1,183 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.Map;
+
+import org.junit.jupiter.api.Test;
+
+import static edu.wpi.first.wpilibj2.command.CommandGroupBase.parallel;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class RobotDisabledCommandTest extends CommandTestBase {
+  @Test
+  void robotDisabledCommandCancelTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder holder = new MockCommandHolder(false);
+    Command mockCommand = holder.getMock();
+
+    scheduler.schedule(mockCommand);
+
+    assertTrue(scheduler.isScheduled(mockCommand));
+
+    setDSEnabled(false);
+
+    scheduler.run();
+
+    assertFalse(scheduler.isScheduled(mockCommand));
+
+    setDSEnabled(true);
+  }
+
+  @Test
+  void runWhenDisabledTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder holder = new MockCommandHolder(true);
+    Command mockCommand = holder.getMock();
+
+    scheduler.schedule(mockCommand);
+
+    assertTrue(scheduler.isScheduled(mockCommand));
+
+    setDSEnabled(false);
+
+    scheduler.run();
+
+    assertTrue(scheduler.isScheduled(mockCommand));
+  }
+
+  @Test
+  void sequentialGroupRunWhenDisabledTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true);
+    Command command2 = command2Holder.getMock();
+    MockCommandHolder command3Holder = new MockCommandHolder(true);
+    Command command3 = command3Holder.getMock();
+    MockCommandHolder command4Holder = new MockCommandHolder(false);
+    Command command4 = command4Holder.getMock();
+
+    Command runWhenDisabled = new SequentialCommandGroup(command1, command2);
+    Command dontRunWhenDisabled = new SequentialCommandGroup(command3, command4);
+
+    scheduler.schedule(runWhenDisabled);
+    scheduler.schedule(dontRunWhenDisabled);
+
+    setDSEnabled(false);
+
+    scheduler.run();
+
+    assertTrue(scheduler.isScheduled(runWhenDisabled));
+    assertFalse(scheduler.isScheduled(dontRunWhenDisabled));
+  }
+
+  @Test
+  void parallelGroupRunWhenDisabledTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true);
+    Command command2 = command2Holder.getMock();
+    MockCommandHolder command3Holder = new MockCommandHolder(true);
+    Command command3 = command3Holder.getMock();
+    MockCommandHolder command4Holder = new MockCommandHolder(false);
+    Command command4 = command4Holder.getMock();
+
+    Command runWhenDisabled = new ParallelCommandGroup(command1, command2);
+    Command dontRunWhenDisabled = new ParallelCommandGroup(command3, command4);
+
+    scheduler.schedule(runWhenDisabled);
+    scheduler.schedule(dontRunWhenDisabled);
+
+    setDSEnabled(false);
+
+    scheduler.run();
+
+    assertTrue(scheduler.isScheduled(runWhenDisabled));
+    assertFalse(scheduler.isScheduled(dontRunWhenDisabled));
+  }
+
+  @Test
+  void conditionalRunWhenDisabledTest() {
+    setDSEnabled(false);
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true);
+    Command command2 = command2Holder.getMock();
+    MockCommandHolder command3Holder = new MockCommandHolder(true);
+    Command command3 = command3Holder.getMock();
+    MockCommandHolder command4Holder = new MockCommandHolder(false);
+    Command command4 = command4Holder.getMock();
+
+    CommandScheduler scheduler = new CommandScheduler();
+
+    Command runWhenDisabled = new ConditionalCommand(command1, command2, () -> true);
+    Command dontRunWhenDisabled = new ConditionalCommand(command3, command4, () -> true);
+
+    scheduler.schedule(runWhenDisabled, dontRunWhenDisabled);
+
+    assertTrue(scheduler.isScheduled(runWhenDisabled));
+    assertFalse(scheduler.isScheduled(dontRunWhenDisabled));
+  }
+
+  @Test
+  void selectRunWhenDisabledTest() {
+    setDSEnabled(false);
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true);
+    Command command2 = command2Holder.getMock();
+    MockCommandHolder command3Holder = new MockCommandHolder(true);
+    Command command3 = command3Holder.getMock();
+    MockCommandHolder command4Holder = new MockCommandHolder(false);
+    Command command4 = command4Holder.getMock();
+
+    Command runWhenDisabled = new SelectCommand(Map.of(1, command1, 2, command2), () -> 1);
+    Command dontRunWhenDisabled = new SelectCommand(Map.of(1, command3, 2, command4), () -> 1);
+
+    CommandScheduler scheduler = new CommandScheduler();
+
+    scheduler.schedule(runWhenDisabled, dontRunWhenDisabled);
+
+    assertTrue(scheduler.isScheduled(runWhenDisabled));
+    assertFalse(scheduler.isScheduled(dontRunWhenDisabled));
+  }
+
+  @Test
+  void parallelConditionalRunWhenDisabledTest() {
+    setDSEnabled(false);
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true);
+    Command command2 = command2Holder.getMock();
+    MockCommandHolder command3Holder = new MockCommandHolder(true);
+    Command command3 = command3Holder.getMock();
+    MockCommandHolder command4Holder = new MockCommandHolder(false);
+    Command command4 = command4Holder.getMock();
+
+    CommandScheduler scheduler = new CommandScheduler();
+
+    Command runWhenDisabled = new ConditionalCommand(command1, command2, () -> true);
+    Command dontRunWhenDisabled = new ConditionalCommand(command3, command4, () -> true);
+
+    Command parallel = parallel(runWhenDisabled, dontRunWhenDisabled);
+
+    scheduler.schedule(parallel);
+
+    assertFalse(scheduler.isScheduled(runWhenDisabled));
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/RunCommandTest.java

@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class RunCommandTest extends CommandTestBase {
+  @Test
+  void runCommandScheduleTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    Counter counter = new Counter();
+
+    RunCommand command = new RunCommand(counter::increment);
+
+    scheduler.schedule(command);
+    scheduler.run();
+    scheduler.run();
+    scheduler.run();
+
+    assertEquals(3, counter.m_counter);
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/ScheduleCommandTest.java

@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import static org.mockito.Mockito.verify;
+
+class ScheduleCommandTest extends CommandTestBase {
+  @Test
+  void scheduleCommandScheduleTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true);
+    Command command2 = command2Holder.getMock();
+
+    ScheduleCommand scheduleCommand = new ScheduleCommand(command1, command2);
+
+    scheduler.schedule(scheduleCommand);
+
+    verify(command1).schedule();
+    verify(command2).schedule();
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/SchedulerTest.java

@@ -0,0 +1,57 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class SchedulerTest extends CommandTestBase {
+  @Test
+  void schedulerLambdaTestNoInterrupt() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    Counter counter = new Counter();
+
+    scheduler.onCommandInitialize(command -> counter.increment());
+    scheduler.onCommandExecute(command -> counter.increment());
+    scheduler.onCommandFinish(command -> counter.increment());
+
+    scheduler.schedule(new InstantCommand());
+    scheduler.run();
+
+    assertEquals(counter.m_counter, 3);
+  }
+
+  @Test
+  void schedulerInterruptLambdaTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    Counter counter = new Counter();
+
+    scheduler.onCommandInterrupt(command -> counter.increment());
+
+    Command command = new WaitCommand(10);
+
+    scheduler.schedule(command);
+    scheduler.cancel(command);
+
+    assertEquals(counter.m_counter, 1);
+  }
+
+  @Test
+  void unregisterSubsystemTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    Subsystem system = new TestSubsystem();
+
+    scheduler.registerSubsystem(system);
+    assertDoesNotThrow(() -> scheduler.unregisterSubsystem(system));
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/SelectCommandTest.java

@@ -0,0 +1,108 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.Map;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.mockito.Mockito.never;
+import static org.mockito.Mockito.verify;
+
+class SelectCommandTest extends CommandTestBase {
+  @Test
+  void selectCommandTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+    command1Holder.setFinished(true);
+    MockCommandHolder command2Holder = new MockCommandHolder(true);
+    Command command2 = command2Holder.getMock();
+    MockCommandHolder command3Holder = new MockCommandHolder(true);
+    Command command3 = command3Holder.getMock();
+
+    SelectCommand selectCommand =
+        new SelectCommand(Map.ofEntries(
+            Map.entry("one", command1),
+            Map.entry("two", command2),
+            Map.entry("three", command3)),
+            () -> "one");
+
+    scheduler.schedule(selectCommand);
+    scheduler.run();
+
+    verify(command1).initialize();
+    verify(command1).execute();
+    verify(command1).end(false);
+
+    verify(command2, never()).initialize();
+    verify(command2, never()).execute();
+    verify(command2, never()).end(false);
+
+    verify(command3, never()).initialize();
+    verify(command3, never()).execute();
+    verify(command3, never()).end(false);
+  }
+
+  @Test
+  void selectCommandInvalidKeyTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+    command1Holder.setFinished(true);
+    MockCommandHolder command2Holder = new MockCommandHolder(true);
+    Command command2 = command2Holder.getMock();
+    MockCommandHolder command3Holder = new MockCommandHolder(true);
+    Command command3 = command3Holder.getMock();
+
+    SelectCommand selectCommand =
+        new SelectCommand(Map.ofEntries(
+            Map.entry("one", command1),
+            Map.entry("two", command2),
+            Map.entry("three", command3)),
+            () -> "four");
+
+    assertDoesNotThrow(() -> scheduler.schedule(selectCommand));
+  }
+
+
+  @Test
+  void selectCommandRequirementTest() {
+    Subsystem system1 = new TestSubsystem();
+    Subsystem system2 = new TestSubsystem();
+    Subsystem system3 = new TestSubsystem();
+    Subsystem system4 = new TestSubsystem();
+
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true, system3);
+    Command command2 = command2Holder.getMock();
+    MockCommandHolder command3Holder = new MockCommandHolder(true, system3, system4);
+    Command command3 = command3Holder.getMock();
+
+    SelectCommand selectCommand = new SelectCommand(
+        Map.ofEntries(Map.entry("one", command1), Map.entry("two", command2),
+            Map.entry("three", command3)), () -> "one");
+
+    scheduler.schedule(selectCommand);
+    scheduler.schedule(new InstantCommand(() -> {
+    }, system3));
+
+    assertFalse(scheduler.isScheduled(selectCommand));
+
+    verify(command1).end(true);
+    verify(command2, never()).end(true);
+    verify(command3, never()).end(true);
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroupTest.java

@@ -0,0 +1,128 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.mockito.Mockito.never;
+import static org.mockito.Mockito.verify;
+
+class SequentialCommandGroupTest extends CommandTestBase {
+  @Test
+  void sequentialGroupScheduleTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true);
+    Command command2 = command2Holder.getMock();
+
+    Command group = new SequentialCommandGroup(command1, command2);
+
+    scheduler.schedule(group);
+
+    verify(command1).initialize();
+    verify(command2, never()).initialize();
+
+    command1Holder.setFinished(true);
+    scheduler.run();
+
+    verify(command1).execute();
+    verify(command1).end(false);
+    verify(command2).initialize();
+    verify(command2, never()).execute();
+    verify(command2, never()).end(false);
+
+    command2Holder.setFinished(true);
+    scheduler.run();
+
+    verify(command1).execute();
+    verify(command1).end(false);
+    verify(command2).execute();
+    verify(command2).end(false);
+
+    assertFalse(scheduler.isScheduled(group));
+  }
+
+  @Test
+  void sequentialGroupInterruptTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true);
+    Command command2 = command2Holder.getMock();
+    MockCommandHolder command3Holder = new MockCommandHolder(true);
+    Command command3 = command3Holder.getMock();
+
+    Command group = new SequentialCommandGroup(command1, command2, command3);
+
+    scheduler.schedule(group);
+
+    command1Holder.setFinished(true);
+    scheduler.run();
+    scheduler.cancel(group);
+    scheduler.run();
+
+    verify(command1).execute();
+    verify(command1, never()).end(true);
+    verify(command1).end(false);
+    verify(command2, never()).execute();
+    verify(command2).end(true);
+    verify(command2, never()).end(false);
+    verify(command3, never()).initialize();
+    verify(command3, never()).execute();
+    verify(command3, never()).end(true);
+    verify(command3, never()).end(false);
+
+    assertFalse(scheduler.isScheduled(group));
+  }
+
+  @Test
+  void notScheduledCancelTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true);
+    Command command2 = command2Holder.getMock();
+
+    Command group = new SequentialCommandGroup(command1, command2);
+
+    assertDoesNotThrow(() -> scheduler.cancel(group));
+  }
+
+
+  @Test
+  void sequentialGroupRequirementTest() {
+    Subsystem system1 = new TestSubsystem();
+    Subsystem system2 = new TestSubsystem();
+    Subsystem system3 = new TestSubsystem();
+    Subsystem system4 = new TestSubsystem();
+
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true, system3);
+    Command command2 = command2Holder.getMock();
+    MockCommandHolder command3Holder = new MockCommandHolder(true, system3, system4);
+    Command command3 = command3Holder.getMock();
+
+    Command group = new SequentialCommandGroup(command1, command2);
+
+    scheduler.schedule(group);
+    scheduler.schedule(command3);
+
+    assertFalse(scheduler.isScheduled(group));
+    assertTrue(scheduler.isScheduled(command3));
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/StartEndCommandTest.java

@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class StartEndCommandTest extends CommandTestBase {
+  @Test
+  void startEndCommandScheduleTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    ConditionHolder cond1 = new ConditionHolder();
+    ConditionHolder cond2 = new ConditionHolder();
+
+    StartEndCommand command =
+        new StartEndCommand(() -> cond1.setCondition(true), () -> cond2.setCondition(true));
+
+    scheduler.schedule(command);
+    scheduler.run();
+
+    assertTrue(scheduler.isScheduled(command));
+
+    scheduler.cancel(command);
+
+    assertFalse(scheduler.isScheduled(command));
+    assertTrue(cond1.getCondition());
+    assertTrue(cond2.getCondition());
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/WaitCommandTest.java

@@ -0,0 +1,67 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.Timer;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.mockito.ArgumentMatchers.anyDouble;
+import static org.mockito.Mockito.never;
+import static org.mockito.Mockito.verify;
+import static org.mockito.Mockito.when;
+
+class WaitCommandTest extends CommandTestBase {
+  @Test
+  void waitCommandTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    WaitCommand waitCommand = new WaitCommand(2);
+
+    scheduler.schedule(waitCommand);
+    scheduler.run();
+    Timer.delay(1);
+    scheduler.run();
+
+    assertTrue(scheduler.isScheduled(waitCommand));
+
+    Timer.delay(2);
+
+    scheduler.run();
+
+    assertFalse(scheduler.isScheduled(waitCommand));
+  }
+
+  @Test
+  void withTimeoutTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+    when(command1.withTimeout(anyDouble())).thenCallRealMethod();
+
+    Command timeout = command1.withTimeout(2);
+
+    scheduler.schedule(timeout);
+    scheduler.run();
+
+    verify(command1).initialize();
+    verify(command1).execute();
+    assertFalse(scheduler.isScheduled(command1));
+    assertTrue(scheduler.isScheduled(timeout));
+
+    Timer.delay(3);
+    scheduler.run();
+
+    verify(command1).end(true);
+    verify(command1, never()).end(false);
+    assertFalse(scheduler.isScheduled(timeout));
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/WaitUntilCommandTest.java

@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class WaitUntilCommandTest extends CommandTestBase {
+  @Test
+  void waitUntilTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    ConditionHolder condition = new ConditionHolder();
+
+    Command command = new WaitUntilCommand(condition::getCondition);
+
+    scheduler.schedule(command);
+    scheduler.run();
+    assertTrue(scheduler.isScheduled(command));
+    condition.setCondition(true);
+    scheduler.run();
+    assertFalse(scheduler.isScheduled(command));
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json

@@ -230,6 +230,16 @@
     "gradlebase": "java",
     "mainclass": "Main"
   },
+  {
+    "name": "GearsBot (New)",
+    "description": "A fully functional example CommandBased program for WPIs GearsBot robot, ported to the new CommandBased library. This code can run on your computer if it supports simulation.",
+    "tags": [
+      "Complete Robot"
+    ],
+    "foldername": "gearsbotnew",
+    "gradlebase": "java",
+    "mainclass": "Main"
+  },
   {
     "name": "PacGoat",
     "description": "A fully functional example CommandBased program for FRC Team 190's 2014 robot. This code can run on your computer if it supports simulation.",
@@ -282,5 +292,66 @@
     "foldername": "shuffleboard",
     "gradlebase": "java",
     "mainclass": "Main"
+  },
+  {
+    "name": "'Traditional' Hatchbot",
+    "description": "A fully-functional command-based hatchbot for the 2019 game using the new experimental command API.  Written in the 'traditional' style, i.e. commands are given their own classes.",
+    "tags": [
+      "Complete robot",
+      "Command-based"
+    ],
+    "foldername": "hatchbottraditional",
+    "mainclass": "Main"
+  },
+  {
+    "name": "'Inlined' Hatchbot",
+    "description": "A fully-functional command-based hatchbot for the 2019 game using the new experimental command API.  Written in the 'inlined' style, i.e. many commands are defined inline with lambdas.",
+    "tags": [
+      "Complete robot",
+      "Command-based",
+      "Lambdas"
+    ],
+    "foldername": "hatchbotinlined",
+    "mainclass": "Main"
+  },
+  {
+    "name": "Select Command Example",
+    "description": "An example showing how to use the SelectCommand class from the experimental command framework rewrite.",
+    "tags": [
+      "Command-based"
+    ],
+    "foldername": "selectcommand",
+    "mainclass": "Main"
+  },
+  {
+    "name": "Scheduler Event Logging",
+    "description": "An example showing how to use Shuffleboard to log Command events from the CommandScheduler in the experimental command framework rewrite",
+    "tags": [
+      "Command-based",
+      "Shuffleboard"
+    ],
+    "foldername": "schedulereventlogging",
+    "mainclass": "Main"
+  },
+  {
+    "name": "Frisbeebot",
+    "description": "An example robot project for a simple frisbee shooter for the 2013 FRC game, Ultimate Ascent, demonstrating use of PID functionality in the command framework",
+    "tags": [
+      "Command-based",
+      "PID"
+    ],
+    "foldername": "frisbeebot",
+    "mainclass": "Main"
+  },
+  {
+    "name": "Gyro Drive Commands",
+    "description": "An example command-based robot project demonstrating simple PID functionality utilizing a gyroscope to keep a robot driving straight and to turn to specified angles.",
+    "tags": [
+      "Command-based",
+      "PID",
+      "Gyro"
+    ],
+    "foldername": "gyrodrivecommands",
+    "mainclass": "Main"
   }
 ]

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Constants.java

@@ -0,0 +1,74 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.frisbeebot;
+
+/**
+ * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
+ * constants.  This class should not be used for any other purpose.  All constants should be
+ * declared globally (i.e. public static).  Do not put anything functional in this class.
+ *
+ * <p>It is advised to statically import this class (or one of its inner classes) wherever the
+ * constants are needed, to reduce verbosity.
+ */
+public final class Constants {
+  public static final class DriveConstants {
+    public static final int kLeftMotor1Port = 0;
+    public static final int kLeftMotor2Port = 1;
+    public static final int kRightMotor1Port = 2;
+    public static final int kRightMotor2Port = 3;
+
+    public static final int[] kLeftEncoderPorts = new int[]{0, 1};
+    public static final int[] kRightEncoderPorts = new int[]{2, 3};
+    public static final boolean kLeftEncoderReversed = false;
+    public static final boolean kRightEncoderReversed = true;
+
+    public static final int kEncoderCPR = 1024;
+    public static final double kWheelDiameterInches = 6;
+    public static final double kEncoderDistancePerPulse =
+        // Assumes the encoders are directly mounted on the wheel shafts
+        (kWheelDiameterInches * Math.PI) / (double) kEncoderCPR;
+  }
+
+  public static final class ShooterConstants {
+    public static final int[] kEncoderPorts = new int[]{4, 5};
+    public static final boolean kEncoderReversed = false;
+    public static final int kEncoderCPR = 1024;
+    public static final double kEncoderDistancePerPulse =
+        // Distance units will be rotations
+        1. / (double) kEncoderCPR;
+
+    public static final int kShooterMotorPort = 4;
+    public static final int kFeederMotorPort = 5;
+
+    public static final double kShooterFreeRPS = 5300;
+    public static final double kShooterTargetRPS = 4000;
+    public static final double kShooterToleranceRPS = 50;
+
+    public static final double kP = 1;
+    public static final double kI = 0;
+    public static final double kD = 0;
+
+    // On a real robot the feedforward constants should be empirically determined; these are
+    // reasonable guesses.
+    public static final double kSFractional = .05;
+    public static final double kVFractional =
+        // Should have value 1 at free speed...
+        1. / kShooterFreeRPS;
+
+    public static final double kFeederSpeed = .5;
+  }
+
+  public static final class AutoConstants {
+    public static final double kAutoTimeoutSeconds = 12;
+    public static final double kAutoShootTimeSeconds = 7;
+  }
+
+  public static final class OIConstants {
+    public static final int kDriverControllerPort = 1;
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Main.java

@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.frisbeebot;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {
+  }
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Robot.java

@@ -0,0 +1,121 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.frisbeebot;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
+/**
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the build.gradle file in the
+ * project.
+ */
+public class Robot extends TimedRobot {
+  private Command m_autonomousCommand;
+
+  private RobotContainer m_robotContainer;
+
+  /**
+   * This function is run when the robot is first started up and should be used for any
+   * initialization code.
+   */
+  @Override
+  public void robotInit() {
+    // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
+    // autonomous chooser on the dashboard.
+    m_robotContainer = new RobotContainer();
+  }
+
+  /**
+   * This function is called every robot packet, no matter the mode. Use this for items like
+   * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
+   *
+   * <p>This runs after the mode specific periodic functions, but before
+   * LiveWindow and SmartDashboard integrated updating.
+   */
+  @Override
+  public void robotPeriodic() {
+    // Runs the Scheduler.  This is responsible for polling buttons, adding newly-scheduled
+    // commands, running already-scheduled commands, removing finished or interrupted commands,
+    // and running subsystem periodic() methods.  This must be called from the robot's periodic
+    // block in order for anything in the Command-based framework to work.
+    CommandScheduler.getInstance().run();
+  }
+
+  /**
+   * This function is called once each time the robot enters Disabled mode.
+   */
+  @Override
+  public void disabledInit() {
+  }
+
+  @Override
+  public void disabledPeriodic() {
+  }
+
+  /**
+   * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
+   */
+  @Override
+  public void autonomousInit() {
+    m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+
+    /*
+     * String autoSelected = SmartDashboard.getString("Auto Selector",
+     * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
+     * = new MyAutoCommand(); break; case "Default Auto": default:
+     * autonomousCommand = new ExampleCommand(); break; }
+     */
+
+    // schedule the autonomous command (example)
+    if (m_autonomousCommand != null) {
+      m_autonomousCommand.schedule();
+    }
+  }
+
+  /**
+   * This function is called periodically during autonomous.
+   */
+  @Override
+  public void autonomousPeriodic() {
+  }
+
+  @Override
+  public void teleopInit() {
+    // This makes sure that the autonomous stops running when
+    // teleop starts running. If you want the autonomous to
+    // continue until interrupted by another command, remove
+    // this line or comment it out.
+    if (m_autonomousCommand != null) {
+      m_autonomousCommand.cancel();
+    }
+  }
+
+  /**
+   * This function is called periodically during operator control.
+   */
+  @Override
+  public void teleopPeriodic() {
+
+  }
+
+  @Override
+  public void testInit() {
+    // Cancels all running commands at the start of test mode.
+    CommandScheduler.getInstance().cancelAll();
+  }
+
+  /**
+   * This function is called periodically during test mode.
+   */
+  @Override
+  public void testPeriodic() {
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/RobotContainer.java

@@ -0,0 +1,119 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.frisbeebot;
+
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.ConditionalCommand;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+
+import edu.wpi.first.wpilibj.examples.frisbeebot.subsystems.DriveSubsystem;
+import edu.wpi.first.wpilibj.examples.frisbeebot.subsystems.ShooterSubsystem;
+
+import static edu.wpi.first.wpilibj.XboxController.Button;
+import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.AutoConstants.kAutoShootTimeSeconds;
+import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.AutoConstants.kAutoTimeoutSeconds;
+import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.OIConstants.kDriverControllerPort;
+
+/**
+ * This class is where the bulk of the robot should be declared.  Since Command-based is a
+ * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
+ * periodic methods (other than the scheduler calls).  Instead, the structure of the robot
+ * (including subsystems, commands, and button mappings) should be declared here.
+ */
+public class RobotContainer {
+  // The robot's subsystems
+  private final DriveSubsystem m_robotDrive = new DriveSubsystem();
+  private final ShooterSubsystem m_shooter = new ShooterSubsystem();
+
+  // A simple autonomous routine that shoots the loaded frisbees
+  private final Command m_autoCommand =
+      // Start the command by spinning up the shooter...
+      new InstantCommand(m_shooter::enable, m_shooter).andThen(
+          // Wait until the shooter is at speed before feeding the frisbees
+          new WaitUntilCommand(m_shooter::atSetpoint),
+          // Start running the feeder
+          new InstantCommand(m_shooter::runFeeder, m_shooter),
+          // Shoot for the specified time
+          new WaitCommand(kAutoShootTimeSeconds))
+          // Add a timeout (will end the command if, for instance, the shooter never gets up to
+          // speed)
+          .withTimeout(kAutoTimeoutSeconds)
+          // When the command ends, turn off the shooter and the feeder
+          .whenFinished(() -> {
+            m_shooter.disable();
+            m_shooter.stopFeeder();
+          });
+
+  // The driver's controller
+  XboxController m_driverController = new XboxController(kDriverControllerPort);
+
+  /**
+   * The container for the robot.  Contains subsystems, OI devices, and commands.
+   */
+  public RobotContainer() {
+    // Configure the button bindings
+    configureButtonBindings();
+
+    // Configure default commands
+    // Set the default drive command to split-stick arcade drive
+    m_robotDrive.setDefaultCommand(
+        // A split-stick arcade command, with forward/backward controlled by the left
+        // hand, and turning controlled by the right.
+        new RunCommand(() -> m_robotDrive
+            .arcadeDrive(m_driverController.getY(GenericHID.Hand.kLeft),
+                m_driverController.getX(GenericHID.Hand.kRight)), m_robotDrive));
+
+  }
+
+  /**
+   * Use this method to define your button->command mappings.  Buttons can be created by
+   * instantiating a {@link GenericHID} or one of its subclasses ({@link
+   * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then calling passing it to a
+   * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+   */
+  private void configureButtonBindings() {
+    // Spin up the shooter when the 'A' button is pressed
+    new JoystickButton(m_driverController, Button.kA.value)
+        .whenPressed(new InstantCommand(m_shooter::enable, m_shooter));
+
+    // Turn off the shooter when the 'B' button is pressed
+    new JoystickButton(m_driverController, Button.kB.value)
+        .whenPressed(new InstantCommand(m_shooter::disable, m_shooter));
+
+    // Run the feeder when the 'X' button is held, but only if the shooter is at speed
+    new JoystickButton(m_driverController, Button.kX.value).whenPressed(new ConditionalCommand(
+        // Run the feeder
+        new InstantCommand(m_shooter::runFeeder, m_shooter),
+        // Do nothing
+        new InstantCommand(),
+        // Determine which of the above to do based on whether the shooter has reached the
+        // desired speed
+        m_shooter::atSetpoint)).whenReleased(new InstantCommand(m_shooter::stopFeeder, m_shooter));
+
+    // Drive at half speed when the bumper is held
+    new JoystickButton(m_driverController, Button.kBumperRight.value)
+        .whenPressed(() -> m_robotDrive.setMaxOutput(.5))
+        .whenReleased(() -> m_robotDrive.setMaxOutput(1));
+  }
+
+
+  /**
+   * Use this to pass the autonomous command to the main {@link Robot} class.
+   *
+   * @return the command to run in autonomous
+   */
+  public Command getAutonomousCommand() {
+    return m_autoCommand;
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/DriveSubsystem.java

@@ -0,0 +1,110 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.frisbeebot.subsystems;
+
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.PWMVictorSPX;
+import edu.wpi.first.wpilibj.SpeedControllerGroup;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kEncoderDistancePerPulse;
+import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kLeftEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kLeftEncoderReversed;
+import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kLeftMotor1Port;
+import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kLeftMotor2Port;
+import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kRightEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kRightEncoderReversed;
+import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kRightMotor1Port;
+import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kRightMotor2Port;
+
+public class DriveSubsystem extends SubsystemBase {
+  // The motors on the left side of the drive.
+  private final SpeedControllerGroup m_leftMotors =
+      new SpeedControllerGroup(new PWMVictorSPX(kLeftMotor1Port),
+          new PWMVictorSPX(kLeftMotor2Port));
+
+  // The motors on the right side of the drive.
+  private final SpeedControllerGroup m_rightMotors =
+      new SpeedControllerGroup(new PWMVictorSPX(kRightMotor1Port),
+          new PWMVictorSPX(kRightMotor2Port));
+
+  // The robot's drive
+  private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
+
+  // The left-side drive encoder
+  private final Encoder m_leftEncoder =
+      new Encoder(kLeftEncoderPorts[0], kLeftEncoderPorts[1], kLeftEncoderReversed);
+
+  // The right-side drive encoder
+  private final Encoder m_rightEncoder =
+      new Encoder(kRightEncoderPorts[0], kRightEncoderPorts[1], kRightEncoderReversed);
+
+  /**
+   * Creates a new DriveSubsystem.
+   */
+  public DriveSubsystem() {
+    // Sets the distance per pulse for the encoders
+    m_leftEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
+    m_rightEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
+  }
+
+  /**
+   * Drives the robot using arcade controls.
+   *
+   * @param fwd the commanded forward movement
+   * @param rot the commanded rotation
+   */
+  public void arcadeDrive(double fwd, double rot) {
+    m_drive.arcadeDrive(fwd, rot);
+  }
+
+  /**
+   * Resets the drive encoders to currently read a position of 0.
+   */
+  public void resetEncoders() {
+    m_leftEncoder.reset();
+    m_rightEncoder.reset();
+  }
+
+  /**
+   * Gets the average distance of the two encoders.
+   *
+   * @return the average of the two encoder readings
+   */
+  public double getAverageEncoderDistance() {
+    return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.;
+  }
+
+  /**
+   * Gets the left drive encoder.
+   *
+   * @return the left drive encoder
+   */
+  public Encoder getLeftEncoder() {
+    return m_leftEncoder;
+  }
+
+  /**
+   * Gets the right drive encoder.
+   *
+   * @return the right drive encoder
+   */
+  public Encoder getRightEncoder() {
+    return m_rightEncoder;
+  }
+
+  /**
+   * Sets the max output of the drive.  Useful for scaling the drive to drive more slowly.
+   *
+   * @param maxOutput the maximum output to which the drive will be constrained
+   */
+  public void setMaxOutput(double maxOutput) {
+    m_drive.setMaxOutput(maxOutput);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/ShooterSubsystem.java

@@ -0,0 +1,79 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.frisbeebot.subsystems;
+
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.PWMVictorSPX;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj2.command.PIDSubsystem;
+
+import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kD;
+import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kEncoderDistancePerPulse;
+import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kEncoderReversed;
+import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kFeederMotorPort;
+import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kFeederSpeed;
+import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kI;
+import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kP;
+import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kSFractional;
+import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kShooterMotorPort;
+import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kShooterTargetRPS;
+import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kShooterToleranceRPS;
+import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kVFractional;
+
+public class ShooterSubsystem extends PIDSubsystem {
+  private final PWMVictorSPX m_shooterMotor = new PWMVictorSPX(kShooterMotorPort);
+  private final PWMVictorSPX m_feederMotor = new PWMVictorSPX(kFeederMotorPort);
+  private final Encoder m_shooterEncoder =
+      new Encoder(kEncoderPorts[0], kEncoderPorts[1], kEncoderReversed);
+
+  /**
+   * The shooter subsystem for the robot.
+   */
+  public ShooterSubsystem() {
+    super(new PIDController(kP, kI, kD));
+    getController().setAbsoluteTolerance(kShooterToleranceRPS);
+    m_shooterEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
+  }
+
+  @Override
+  public void useOutput(double output) {
+    // Use a feedforward of the form kS + kV * velocity
+    m_shooterMotor.set(output + kSFractional + kVFractional * kShooterTargetRPS);
+  }
+
+  @Override
+  public double getSetpoint() {
+    return kShooterTargetRPS;
+  }
+
+  @Override
+  public double getMeasurement() {
+    return m_shooterEncoder.getRate();
+  }
+
+  public boolean atSetpoint() {
+    return m_controller.atSetpoint();
+  }
+
+  public void runFeeder() {
+    m_feederMotor.set(kFeederSpeed);
+  }
+
+  public void stopFeeder() {
+    m_feederMotor.set(0);
+  }
+
+  @Override
+  public void disable() {
+    super.disable();
+    // Turn off motor when we disable, since useOutput(0) doesn't stop the motor due to our
+    // feedforward
+    m_shooterMotor.set(0);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/Main.java

@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.gearsbotnew;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {
+  }
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/Robot.java

@@ -0,0 +1,113 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.gearsbotnew;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
+/**
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the build.gradle file in the
+ * project.
+ */
+public class Robot extends TimedRobot {
+  private Command m_autonomousCommand;
+
+  private RobotContainer m_robotContainer;
+
+  /**
+   * This function is run when the robot is first started up and should be used for any
+   * initialization code.
+   */
+  @Override
+  public void robotInit() {
+    // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
+    // autonomous chooser on the dashboard.
+    m_robotContainer = new RobotContainer();
+  }
+
+  /**
+   * This function is called every robot packet, no matter the mode. Use this for items like
+   * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
+   *
+   * <p>This runs after the mode specific periodic functions, but before
+   * LiveWindow and SmartDashboard integrated updating.
+   */
+  @Override
+  public void robotPeriodic() {
+    // Runs the Scheduler.  This is responsible for polling buttons, adding newly-scheduled
+    // commands, running already-scheduled commands, removing finished or interrupted commands,
+    // and running subsystem periodic() methods.  This must be called from the robot's periodic
+    // block in order for anything in the Command-based framework to work.
+    CommandScheduler.getInstance().run();
+  }
+
+  /**
+   * This function is called once each time the robot enters Disabled mode.
+   */
+  @Override
+  public void disabledInit() {
+  }
+
+  @Override
+  public void disabledPeriodic() {
+  }
+
+  /**
+   * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
+   */
+  @Override
+  public void autonomousInit() {
+    m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+
+    // schedule the autonomous command (example)
+    if (m_autonomousCommand != null) {
+      m_autonomousCommand.schedule();
+    }
+  }
+
+  /**
+   * This function is called periodically during autonomous.
+   */
+  @Override
+  public void autonomousPeriodic() {
+  }
+
+  @Override
+  public void teleopInit() {
+    // This makes sure that the autonomous stops running when
+    // teleop starts running. If you want the autonomous to
+    // continue until interrupted by another command, remove
+    // this line or comment it out.
+    if (m_autonomousCommand != null) {
+      m_autonomousCommand.cancel();
+    }
+  }
+
+  /**
+   * This function is called periodically during operator control.
+   */
+  @Override
+  public void teleopPeriodic() {
+  }
+
+  @Override
+  public void testInit() {
+    // Cancels all running commands at the start of test mode.
+    CommandScheduler.getInstance().cancelAll();
+  }
+
+  /**
+   * This function is called periodically during test mode.
+   */
+  @Override
+  public void testPeriodic() {
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/RobotContainer.java

@@ -0,0 +1,127 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.gearsbotnew;
+
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.GenericHID.Hand;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+
+import edu.wpi.first.wpilibj.examples.gearsbotnew.commands.Autonomous;
+import edu.wpi.first.wpilibj.examples.gearsbotnew.commands.CloseClaw;
+import edu.wpi.first.wpilibj.examples.gearsbotnew.commands.OpenClaw;
+import edu.wpi.first.wpilibj.examples.gearsbotnew.commands.Pickup;
+import edu.wpi.first.wpilibj.examples.gearsbotnew.commands.Place;
+import edu.wpi.first.wpilibj.examples.gearsbotnew.commands.PrepareToPickup;
+import edu.wpi.first.wpilibj.examples.gearsbotnew.commands.SetElevatorSetpoint;
+import edu.wpi.first.wpilibj.examples.gearsbotnew.commands.SetWristSetpoint;
+import edu.wpi.first.wpilibj.examples.gearsbotnew.commands.TankDrive;
+import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Claw;
+import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.DriveTrain;
+import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Elevator;
+import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Wrist;
+
+/**
+ * This class is where the bulk of the robot should be declared.  Since Command-based is a
+ * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
+ * periodic methods (other than the scheduler calls).  Instead, the structure of the robot
+ * (including subsystems, commands, and button mappings) should be declared here.
+ */
+public class RobotContainer {
+  // The robot's subsystems and commands are defined here...
+  private final DriveTrain m_drivetrain = new DriveTrain();
+  private final Elevator m_elevator = new Elevator();
+  private final Wrist m_wrist = new Wrist();
+  private final Claw m_claw = new Claw();
+
+  private final Joystick m_joystick = new Joystick(0);
+
+  private final CommandBase m_autonomousCommand =
+      new Autonomous(m_drivetrain, m_claw, m_wrist, m_elevator);
+
+  /**
+   * The container for the robot.  Contains subsystems, OI devices, and commands.
+   */
+  public RobotContainer() {
+    // Put Some buttons on the SmartDashboard
+    SmartDashboard.putData("Elevator Bottom", new SetElevatorSetpoint(0, m_elevator));
+    SmartDashboard.putData("Elevator Platform", new SetElevatorSetpoint(0.2, m_elevator));
+    SmartDashboard.putData("Elevator Top", new SetElevatorSetpoint(0.3, m_elevator));
+
+    SmartDashboard.putData("Wrist Horizontal", new SetWristSetpoint(0, m_wrist));
+    SmartDashboard.putData("Raise Wrist", new SetWristSetpoint(-45, m_wrist));
+
+    SmartDashboard.putData("Open Claw", new OpenClaw(m_claw));
+    SmartDashboard.putData("Close Claw", new CloseClaw(m_claw));
+
+    SmartDashboard
+        .putData("Deliver Soda", new Autonomous(m_drivetrain, m_claw, m_wrist, m_elevator));
+
+    // Assign default commands
+    m_drivetrain.setDefaultCommand(new TankDrive(m_drivetrain, () -> m_joystick.getY(Hand.kLeft),
+        () -> m_joystick.getY(Hand.kRight)));
+
+    // Show what command your subsystem is running on the SmartDashboard
+    SmartDashboard.putData(m_drivetrain);
+    SmartDashboard.putData(m_elevator);
+    SmartDashboard.putData(m_wrist);
+    SmartDashboard.putData(m_claw);
+
+    // Call log method on all subsystems
+    m_wrist.log();
+    m_elevator.log();
+    m_drivetrain.log();
+    m_claw.log();
+
+    // Configure the button bindings
+    configureButtonBindings();
+  }
+
+  /**
+   * Use this method to define your button->command mappings.  Buttons can be created by
+   * instantiating a {@link GenericHID} or one of its subclasses ({@link
+   * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then calling passing it to a
+   * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+   */
+  private void configureButtonBindings() {
+    // Create some buttons
+    final JoystickButton dpadUp = new JoystickButton(m_joystick, 5);
+    final JoystickButton dpadRight = new JoystickButton(m_joystick, 6);
+    final JoystickButton dpadDown = new JoystickButton(m_joystick, 7);
+    final JoystickButton dpadLeft = new JoystickButton(m_joystick, 8);
+    final JoystickButton l2 = new JoystickButton(m_joystick, 9);
+    final JoystickButton r2 = new JoystickButton(m_joystick, 10);
+    final JoystickButton l1 = new JoystickButton(m_joystick, 11);
+    final JoystickButton r1 = new JoystickButton(m_joystick, 12);
+
+    // Connect the buttons to commands
+    dpadUp.whenPressed(new SetElevatorSetpoint(0.2, m_elevator));
+    dpadDown.whenPressed(new SetElevatorSetpoint(-0.2, m_elevator));
+    dpadRight.whenPressed(new CloseClaw(m_claw));
+    dpadLeft.whenPressed(new OpenClaw(m_claw));
+
+    r1.whenPressed(new PrepareToPickup(m_claw, m_wrist, m_elevator));
+    r2.whenPressed(new Pickup(m_claw, m_wrist, m_elevator));
+    l1.whenPressed(new Place(m_claw, m_wrist, m_elevator));
+    l2.whenPressed(new Autonomous(m_drivetrain, m_claw, m_wrist, m_elevator));
+  }
+
+
+  /**
+   * Use this to pass the autonomous command to the main {@link Robot} class.
+   *
+   * @return the command to run in autonomous
+   */
+  public Command getAutonomousCommand() {
+    return m_autonomousCommand;
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/Autonomous.java

@@ -0,0 +1,39 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.gearsbotnew.commands;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+
+import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Claw;
+import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.DriveTrain;
+import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Elevator;
+import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Wrist;
+
+/**
+ * The main autonomous command to pickup and deliver the soda to the box.
+ */
+public class Autonomous extends SequentialCommandGroup {
+  /**
+   * Create a new autonomous command.
+   */
+  public Autonomous(DriveTrain drive, Claw claw, Wrist wrist, Elevator elevator) {
+    addCommands(
+        new PrepareToPickup(claw, wrist, elevator),
+        new Pickup(claw, wrist, elevator),
+        new SetDistanceToBox(0.10, drive),
+        // new DriveStraight(4), // Use encoders if ultrasonic is broken
+        new Place(claw, wrist, elevator),
+        new SetDistanceToBox(0.60, drive),
+        // new DriveStraight(-2), // Use Encoders if ultrasonic is broken
+        parallel(
+            new SetWristSetpoint(-45, wrist),
+            new CloseClaw(claw)
+        )
+    );
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/CloseClaw.java

@@ -0,0 +1,49 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.gearsbotnew.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+import edu.wpi.first.wpilibj.examples.gearsbotnew.Robot;
+import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Claw;
+
+/**
+ * Closes the claw for one second. Real robots should use sensors, stalling motors is BAD!
+ */
+public class CloseClaw extends CommandBase {
+  private final Claw m_claw;
+
+  public CloseClaw(Claw claw) {
+    m_claw = claw;
+    addRequirements(m_claw);
+  }
+
+  // Called just before this Command runs the first time
+  @Override
+  public void initialize() {
+    m_claw.close();
+  }
+
+  // Make this return true when this Command no longer needs to run execute()
+  @Override
+  public boolean isFinished() {
+    return m_claw.isGrabbing();
+  }
+
+  // Called once after isFinished returns true
+  @Override
+  public void end(boolean interrupted) {
+    // NOTE: Doesn't stop in simulation due to lower friction causing the
+    // can to fall out
+    // + there is no need to worry about stalling the motor or crushing the
+    // can.
+    if (!Robot.isSimulation()) {
+      m_claw.stop();
+    }
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/DriveStraight.java

@@ -0,0 +1,53 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.gearsbotnew.commands;
+
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj2.command.PIDCommand;
+
+import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.DriveTrain;
+
+/**
+ * Drive the given distance straight (negative values go backwards). Uses a
+ * local PID controller to run a simple PID loop that is only enabled while this
+ * command is running. The input is the averaged values of the left and right
+ * encoders.
+ */
+public class DriveStraight extends PIDCommand {
+  private final DriveTrain m_drivetrain;
+
+  /**
+   * Create a new DriveStraight command.
+   * @param distance The distance to drive
+   */
+  public DriveStraight(double distance, DriveTrain drivetrain) {
+    super(new PIDController(4, 0, 0),
+        drivetrain::getDistance,
+        distance,
+        d -> drivetrain.drive(d, d));
+
+    m_drivetrain = drivetrain;
+    addRequirements(m_drivetrain);
+
+    getController().setAbsoluteTolerance(0.01);
+  }
+
+  // Called just before this Command runs the first time
+  @Override
+  public void initialize() {
+    // Get everything in a safe starting state.
+    m_drivetrain.reset();
+    super.initialize();
+  }
+
+  // Make this return true when this Command no longer needs to run execute()
+  @Override
+  public boolean isFinished() {
+    return getController().atSetpoint();
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/OpenClaw.java

@@ -0,0 +1,43 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.gearsbotnew.commands;
+
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+
+import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Claw;
+
+/**
+ * Opens the claw for one second. Real robots should use sensors, stalling motors is BAD!
+ */
+public class OpenClaw extends WaitCommand {
+  private final Claw m_claw;
+
+  /**
+   * Creates a new OpenClaw command.
+   *
+   * @param claw The claw to use
+   */
+  public OpenClaw(Claw claw) {
+    super(1);
+    m_claw = claw;
+    addRequirements(m_claw);
+  }
+
+  // Called just before this Command runs the first time
+  @Override
+  public void initialize() {
+    m_claw.open();
+    super.initialize();
+  }
+
+  // Called once after isFinished returns true
+  @Override
+  public void end(boolean interrupted) {
+    m_claw.stop();
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/Pickup.java

@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.gearsbotnew.commands;
+
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+
+import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Claw;
+import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Elevator;
+import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Wrist;
+
+/**
+ * Pickup a soda can (if one is between the open claws) and get it in a safe state to drive around.
+ */
+public class Pickup extends SequentialCommandGroup {
+  /**
+   * Create a new pickup command.
+   *
+   * @param claw     The claw subsystem to use
+   * @param wrist    The wrist subsystem to use
+   * @param elevator The elevator subsystem to use
+   */
+  public Pickup(Claw claw, Wrist wrist, Elevator elevator) {
+    addCommands(
+        new CloseClaw(claw),
+        parallel(
+            new SetWristSetpoint(-45, wrist),
+            new SetElevatorSetpoint(0.25, elevator)));
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/Place.java

@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.gearsbotnew.commands;
+
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+
+import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Claw;
+import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Elevator;
+import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Wrist;
+
+/**
+ * Place a held soda can onto the platform.
+ */
+public class Place extends SequentialCommandGroup {
+  /**
+   * Create a new place command.
+   *
+   * @param claw     The claw subsystem to use
+   * @param wrist    The wrist subsystem to use
+   * @param elevator The elevator subsystem to use
+   */
+  public Place(Claw claw, Wrist wrist, Elevator elevator) {
+    addCommands(
+        new SetElevatorSetpoint(0.25, elevator),
+        new SetWristSetpoint(0, wrist),
+        new OpenClaw(claw));
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/PrepareToPickup.java

@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.gearsbotnew.commands;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+
+import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Claw;
+import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Elevator;
+import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Wrist;
+
+/**
+ * Make sure the robot is in a state to pickup soda cans.
+ */
+public class PrepareToPickup extends SequentialCommandGroup {
+  /**
+   * Create a new prepare to pickup command.
+   *
+   * @param claw     The claw subsystem to use
+   * @param wrist    The wrist subsystem to use
+   * @param elevator The elevator subsystem to use
+   */
+  public PrepareToPickup(Claw claw, Wrist wrist, Elevator elevator) {
+    addCommands(
+        new OpenClaw(claw),
+        parallel(
+            new SetWristSetpoint(0, wrist),
+            new SetElevatorSetpoint(0, elevator)));
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/SetDistanceToBox.java

@@ -0,0 +1,54 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.gearsbotnew.commands;
+
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj2.command.PIDCommand;
+
+import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.DriveTrain;
+
+
+/**
+ * Drive until the robot is the given distance away from the box. Uses a local
+ * PID controller to run a simple PID loop that is only enabled while this
+ * command is running. The input is the averaged values of the left and right
+ * encoders.
+ */
+public class SetDistanceToBox extends PIDCommand {
+  private final DriveTrain m_drivetrain;
+
+  /**
+   * Create a new set distance to box command.
+   *
+   * @param distance The distance away from the box to drive to
+   */
+  public SetDistanceToBox(double distance, DriveTrain drivetrain) {
+    super(new PIDController(-2, 0, 0),
+        drivetrain::getDistanceToObstacle, distance,
+        d -> drivetrain.drive(d, d));
+
+    m_drivetrain = drivetrain;
+    addRequirements(m_drivetrain);
+
+    getController().setAbsoluteTolerance(0.01);
+  }
+
+  // Called just before this Command runs the first time
+  @Override
+  public void initialize() {
+    // Get everything in a safe starting state.
+    m_drivetrain.reset();
+    super.initialize();
+  }
+
+  // Make this return true when this Command no longer needs to run execute()
+  @Override
+  public boolean isFinished() {
+    return getController().atSetpoint();
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/SetElevatorSetpoint.java

@@ -0,0 +1,48 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.gearsbotnew.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Elevator;
+
+
+/**
+ * Move the elevator to a given location. This command finishes when it is within the tolerance, but
+ * leaves the PID loop running to maintain the position. Other commands using the elevator should
+ * make sure they disable PID!
+ */
+public class SetElevatorSetpoint extends CommandBase {
+  private final Elevator m_elevator;
+  private final double m_setpoint;
+
+  /**
+   * Create a new SetElevatorSetpoint command.
+   *
+   * @param setpoint The setpoint to set the elevator to
+   * @param elevator The elevator to use
+   */
+  public SetElevatorSetpoint(double setpoint, Elevator elevator) {
+    m_elevator = elevator;
+    m_setpoint = setpoint;
+    addRequirements(m_elevator);
+  }
+
+  // Called just before this Command runs the first time
+  @Override
+  public void initialize() {
+    m_elevator.setSetpoint(m_setpoint);
+    m_elevator.enable();
+  }
+
+  // Make this return true when this Command no longer needs to run execute()
+  @Override
+  public boolean isFinished() {
+    return m_elevator.getController().atSetpoint();
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/SetWristSetpoint.java

@@ -0,0 +1,48 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.gearsbotnew.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Wrist;
+
+
+/**
+ * Move the wrist to a given angle. This command finishes when it is within the tolerance, but
+ * leaves the PID loop running to maintain the position. Other commands using the wrist should make
+ * sure they disable PID!
+ */
+public class SetWristSetpoint extends CommandBase {
+  private final Wrist m_wrist;
+  private final double m_setpoint;
+
+  /**
+   * Create a new SetWristSetpoint command.
+   *
+   * @param setpoint The setpoint to set the wrist to
+   * @param wrist    The wrist to use
+   */
+  public SetWristSetpoint(double setpoint, Wrist wrist) {
+    m_wrist = wrist;
+    m_setpoint = setpoint;
+    addRequirements(m_wrist);
+  }
+
+  // Called just before this Command runs the first time
+  @Override
+  public void initialize() {
+    m_wrist.enable();
+    m_wrist.setSetpoint(m_setpoint);
+  }
+
+  // Make this return true when this Command no longer needs to run execute()
+  @Override
+  public boolean isFinished() {
+    return m_wrist.getController().atSetpoint();
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/TankDrive.java

@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.gearsbotnew.commands;
+
+
+import java.util.function.DoubleSupplier;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.DriveTrain;
+
+/**
+ * Have the robot drive tank style.
+ */
+public class TankDrive extends CommandBase {
+  private final DriveTrain m_drivetrain;
+  private final DoubleSupplier m_left;
+  private final DoubleSupplier m_right;
+
+  /**
+   * Creates a new TankDrive command.
+   *
+   * @param drivetrain The drivetrain subsystem to drive
+   * @param left       The control input for the left side of the drive
+   * @param right      The control input for the right sight of the drive
+   */
+  public TankDrive(DriveTrain drivetrain, DoubleSupplier left, DoubleSupplier right) {
+    m_drivetrain = drivetrain;
+    m_left = left;
+    m_right = right;
+    addRequirements(m_drivetrain);
+  }
+
+  // Called repeatedly when this Command is scheduled to run
+  @Override
+  public void execute() {
+    m_drivetrain.drive(m_left.getAsDouble(), m_right.getAsDouble());
+  }
+
+  // Make this return true when this Command no longer needs to run execute()
+  @Override
+  public boolean isFinished() {
+    return false; // Runs until interrupted
+  }
+
+  // Called once after isFinished returns true
+  @Override
+  public void end(boolean interrupted) {
+    m_drivetrain.drive(0, 0);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/subsystems/Claw.java

@@ -0,0 +1,63 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.Victor;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+/**
+ * The claw subsystem is a simple system with a motor for opening and closing. If using stronger
+ * motors, you should probably use a sensor so that the motors don't stall.
+ */
+public class Claw extends SubsystemBase {
+  private final Victor m_motor = new Victor(7);
+  private final DigitalInput m_contact = new DigitalInput(5);
+
+  /**
+   * Create a new claw subsystem.
+   */
+  public Claw() {
+    // Let's name everything on the LiveWindow
+    addChild("Motor", m_motor);
+    addChild("Limit Switch", m_contact);
+  }
+
+  public void log() {
+    SmartDashboard.putData("Claw switch", m_contact);
+  }
+
+  /**
+   * Set the claw motor to move in the open direction.
+   */
+  public void open() {
+    m_motor.set(-1);
+  }
+
+  /**
+   * Set the claw motor to move in the close direction.
+   */
+  public void close() {
+    m_motor.set(1);
+  }
+
+  /**
+   * Stops the claw motor from moving.
+   */
+  public void stop() {
+    m_motor.set(0);
+  }
+
+  /**
+   * Return true when the robot is grabbing an object hard enough to trigger the limit switch.
+   */
+  public boolean isGrabbing() {
+    return m_contact.get();
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/subsystems/DriveTrain.java

@@ -0,0 +1,124 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems;
+
+import edu.wpi.first.wpilibj.AnalogGyro;
+import edu.wpi.first.wpilibj.AnalogInput;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.PWMVictorSPX;
+import edu.wpi.first.wpilibj.SpeedController;
+import edu.wpi.first.wpilibj.SpeedControllerGroup;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
+
+public class DriveTrain extends SubsystemBase {
+  /**
+   * The DriveTrain subsystem incorporates the sensors and actuators attached to the robots chassis.
+   * These include four drive motors, a left and right encoder and a gyro.
+   */
+  private final SpeedController m_leftMotor =
+      new SpeedControllerGroup(new PWMVictorSPX(0), new PWMVictorSPX(1));
+  private final SpeedController m_rightMotor =
+      new SpeedControllerGroup(new PWMVictorSPX(2), new PWMVictorSPX(3));
+
+  private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotor, m_rightMotor);
+
+  private final Encoder m_leftEncoder = new Encoder(1, 2);
+  private final Encoder m_rightEncoder = new Encoder(3, 4);
+  private final AnalogInput m_rangefinder = new AnalogInput(6);
+  private final AnalogGyro m_gyro = new AnalogGyro(1);
+
+  /**
+   * Create a new drive train subsystem.
+   */
+  public DriveTrain() {
+    super();
+
+    // Encoders may measure differently in the real world and in
+    // simulation. In this example the robot moves 0.042 barleycorns
+    // per tick in the real world, but the simulated encoders
+    // simulate 360 tick encoders. This if statement allows for the
+    // real robot to handle this difference in devices.
+    if (Robot.isReal()) {
+      m_leftEncoder.setDistancePerPulse(0.042);
+      m_rightEncoder.setDistancePerPulse(0.042);
+    } else {
+      // Circumference in ft = 4in/12(in/ft)*PI
+      m_leftEncoder.setDistancePerPulse((4.0 / 12.0 * Math.PI) / 360.0);
+      m_rightEncoder.setDistancePerPulse((4.0 / 12.0 * Math.PI) / 360.0);
+    }
+
+    // Let's name the sensors on the LiveWindow
+    addChild("Drive", m_drive);
+    addChild("Left Encoder", m_leftEncoder);
+    addChild("Right Encoder", m_rightEncoder);
+    addChild("Rangefinder", m_rangefinder);
+    addChild("Gyro", m_gyro);
+  }
+
+  /**
+   * The log method puts interesting information to the SmartDashboard.
+   */
+  public void log() {
+    SmartDashboard.putNumber("Left Distance", m_leftEncoder.getDistance());
+    SmartDashboard.putNumber("Right Distance", m_rightEncoder.getDistance());
+    SmartDashboard.putNumber("Left Speed", m_leftEncoder.getRate());
+    SmartDashboard.putNumber("Right Speed", m_rightEncoder.getRate());
+    SmartDashboard.putNumber("Gyro", m_gyro.getAngle());
+  }
+
+  /**
+   * Tank style driving for the DriveTrain.
+   *
+   * @param left  Speed in range [-1,1]
+   * @param right Speed in range [-1,1]
+   */
+  public void drive(double left, double right) {
+    m_drive.tankDrive(left, right);
+  }
+
+  /**
+   * Get the robot's heading.
+   *
+   * @return The robots heading in degrees.
+   */
+  public double getHeading() {
+    return m_gyro.getAngle();
+  }
+
+  /**
+   * Reset the robots sensors to the zero states.
+   */
+  public void reset() {
+    m_gyro.reset();
+    m_leftEncoder.reset();
+    m_rightEncoder.reset();
+  }
+
+  /**
+   * Get the average distance of the encoders since the last reset.
+   *
+   * @return The distance driven (average of left and right encoders).
+   */
+  public double getDistance() {
+    return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2;
+  }
+
+  /**
+   * Get the distance to the obstacle.
+   *
+   * @return The distance to the obstacle detected by the rangefinder.
+   */
+  public double getDistanceToObstacle() {
+    // Really meters in simulation since it's a rangefinder...
+    return m_rangefinder.getAverageVoltage();
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/subsystems/Elevator.java

@@ -0,0 +1,98 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems;
+
+import edu.wpi.first.wpilibj.AnalogPotentiometer;
+import edu.wpi.first.wpilibj.Victor;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.PIDSubsystem;
+
+import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
+
+/**
+ * The elevator subsystem uses PID to go to a given height. Unfortunately, in it's current state PID
+ * values for simulation are different than in the real world do to minor differences.
+ */
+public class Elevator extends PIDSubsystem {
+  private final Victor m_motor;
+  private final AnalogPotentiometer m_pot;
+  private double m_setpoint;
+
+  private static final double kP_real = 4;
+  private static final double kI_real = 0.07;
+  private static final double kP_simulation = 18;
+  private static final double kI_simulation = 0.2;
+
+  /**
+   * Create a new elevator subsystem.
+   */
+  public Elevator() {
+    super(new PIDController(kP_real, kI_real, 0));
+    if (Robot.isSimulation()) { // Check for simulation and update PID values
+      getController().setPID(kP_simulation, kI_simulation, 0);
+    }
+    getController().setAbsoluteTolerance(0.005);
+
+    m_motor = new Victor(5);
+
+    // Conversion value of potentiometer varies between the real world and
+    // simulation
+    if (Robot.isReal()) {
+      m_pot = new AnalogPotentiometer(2, -2.0 / 5);
+    } else {
+      m_pot = new AnalogPotentiometer(2); // Defaults to meters
+    }
+
+    // Let's name everything on the LiveWindow
+    addChild("Motor", m_motor);
+    addChild("Pot", m_pot);
+  }
+
+  /**
+   * The log method puts interesting information to the SmartDashboard.
+   */
+  public void log() {
+    SmartDashboard.putData("Elevator Pot", m_pot);
+  }
+
+  /**
+   * Use the potentiometer as the PID sensor. This method is automatically called by the subsystem.
+   */
+  @Override
+  public double getMeasurement() {
+    return m_pot.get();
+  }
+
+  /**
+   * Use the motor as the PID output. This method is automatically called by the subsystem.
+   */
+  @Override
+  public void useOutput(double output) {
+    m_motor.set(output);
+  }
+
+  /**
+   * Returns the setpoint used by the PIDController.
+   *
+   * @return The setpoint for the PIDController.
+   */
+  @Override
+  public double getSetpoint() {
+    return m_setpoint;
+  }
+
+  /**
+   * Sets the setpoint used by the PIDController.
+   *
+   * @param setpoint The setpoint for the PIDController.
+   */
+  public void setSetpoint(double setpoint) {
+    m_setpoint = setpoint;
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/subsystems/Wrist.java

@@ -0,0 +1,95 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems;
+
+import edu.wpi.first.wpilibj.AnalogPotentiometer;
+import edu.wpi.first.wpilibj.Victor;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.PIDSubsystem;
+
+import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
+
+/**
+ * The wrist subsystem is like the elevator, but with a rotational joint instead of a linear joint.
+ */
+public class Wrist extends PIDSubsystem {
+  private final Victor m_motor;
+  private final AnalogPotentiometer m_pot;
+  private double m_setpoint;
+
+  private static final double kP_real = 1;
+  private static final double kP_simulation = 0.05;
+
+  /**
+   * Create a new wrist subsystem.
+   */
+  public Wrist() {
+    super(new PIDController(kP_real, 0, 0));
+    if (Robot.isSimulation()) { // Check for simulation and update PID values
+      getController().setPID(kP_simulation, 0, 0);
+    }
+    getController().setAbsoluteTolerance(2.5);
+
+    m_motor = new Victor(6);
+
+    // Conversion value of potentiometer varies between the real world and
+    // simulation
+    if (Robot.isReal()) {
+      m_pot = new AnalogPotentiometer(3, -270.0 / 5);
+    } else {
+      m_pot = new AnalogPotentiometer(3); // Defaults to degrees
+    }
+
+    // Let's name everything on the LiveWindow
+    addChild("Motor", m_motor);
+    addChild("Pot", m_pot);
+  }
+
+  /**
+   * The log method puts interesting information to the SmartDashboard.
+   */
+  public void log() {
+    SmartDashboard.putData("Wrist Angle", m_pot);
+  }
+
+  /**
+   * Use the potentiometer as the PID sensor. This method is automatically called by the subsystem.
+   */
+  @Override
+  public double getMeasurement() {
+    return m_pot.get();
+  }
+
+  /**
+   * Use the motor as the PID output. This method is automatically called by the subsystem.
+   */
+  @Override
+  public void useOutput(double output) {
+    m_motor.set(output);
+  }
+
+  /**
+   * Returns the setpoint used by the PIDController.
+   *
+   * @return The setpoint for the PIDController.
+   */
+  @Override
+  public double getSetpoint() {
+    return m_setpoint;
+  }
+
+  /**
+   * Sets the setpoint used by the PIDController.
+   *
+   * @param setpoint The setpoint for the PIDController.
+   */
+  public void setSetpoint(double setpoint) {
+    m_setpoint = setpoint;
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Constants.java

@@ -0,0 +1,53 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.gyrodrivecommands;
+
+/**
+ * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
+ * constants.  This class should not be used for any other purpose.  All constants should be
+ * declared globally (i.e. public static).  Do not put anything functional in this class.
+ *
+ * <p>It is advised to statically import this class (or one of its inner classes) wherever the
+ * constants are needed, to reduce verbosity.
+ */
+public final class Constants {
+  public static final class DriveConstants {
+    public static final int kLeftMotor1Port = 0;
+    public static final int kLeftMotor2Port = 1;
+    public static final int kRightMotor1Port = 2;
+    public static final int kRightMotor2Port = 3;
+
+    public static final int[] kLeftEncoderPorts = new int[]{0, 1};
+    public static final int[] kRightEncoderPorts = new int[]{2, 3};
+    public static final boolean kLeftEncoderReversed = false;
+    public static final boolean kRightEncoderReversed = true;
+
+    public static final int kEncoderCPR = 1024;
+    public static final double kWheelDiameterInches = 6;
+    public static final double kEncoderDistancePerPulse =
+        // Assumes the encoders are directly mounted on the wheel shafts
+        (kWheelDiameterInches * Math.PI) / (double) kEncoderCPR;
+
+    public static final boolean kGyroReversed = false;
+
+    public static final double kStabilizationP = 1;
+    public static final double kStabilizationI = .5;
+    public static final double kStabilizationD = 0;
+
+    public static final double kTurnP = 1;
+    public static final double kTurnI = 0;
+    public static final double kTurnD = 0;
+
+    public static final double kTurnToleranceDeg = 5;
+    public static final double kTurnRateToleranceDegPerS = 10; // degrees per second
+  }
+
+  public static final class OIConstants {
+    public static final int kDriverControllerPort = 1;
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Main.java

@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.gyrodrivecommands;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {
+  }
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Robot.java

@@ -0,0 +1,121 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.gyrodrivecommands;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
+/**
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the build.gradle file in the
+ * project.
+ */
+public class Robot extends TimedRobot {
+  private Command m_autonomousCommand;
+
+  private RobotContainer m_robotContainer;
+
+  /**
+   * This function is run when the robot is first started up and should be used for any
+   * initialization code.
+   */
+  @Override
+  public void robotInit() {
+    // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
+    // autonomous chooser on the dashboard.
+    m_robotContainer = new RobotContainer();
+  }
+
+  /**
+   * This function is called every robot packet, no matter the mode. Use this for items like
+   * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
+   *
+   * <p>This runs after the mode specific periodic functions, but before
+   * LiveWindow and SmartDashboard integrated updating.
+   */
+  @Override
+  public void robotPeriodic() {
+    // Runs the Scheduler.  This is responsible for polling buttons, adding newly-scheduled
+    // commands, running already-scheduled commands, removing finished or interrupted commands,
+    // and running subsystem periodic() methods.  This must be called from the robot's periodic
+    // block in order for anything in the Command-based framework to work.
+    CommandScheduler.getInstance().run();
+  }
+
+  /**
+   * This function is called once each time the robot enters Disabled mode.
+   */
+  @Override
+  public void disabledInit() {
+  }
+
+  @Override
+  public void disabledPeriodic() {
+  }
+
+  /**
+   * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
+   */
+  @Override
+  public void autonomousInit() {
+    m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+
+    /*
+     * String autoSelected = SmartDashboard.getString("Auto Selector",
+     * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
+     * = new MyAutoCommand(); break; case "Default Auto": default:
+     * autonomousCommand = new ExampleCommand(); break; }
+     */
+
+    // schedule the autonomous command (example)
+    if (m_autonomousCommand != null) {
+      m_autonomousCommand.schedule();
+    }
+  }
+
+  /**
+   * This function is called periodically during autonomous.
+   */
+  @Override
+  public void autonomousPeriodic() {
+  }
+
+  @Override
+  public void teleopInit() {
+    // This makes sure that the autonomous stops running when
+    // teleop starts running. If you want the autonomous to
+    // continue until interrupted by another command, remove
+    // this line or comment it out.
+    if (m_autonomousCommand != null) {
+      m_autonomousCommand.cancel();
+    }
+  }
+
+  /**
+   * This function is called periodically during operator control.
+   */
+  @Override
+  public void teleopPeriodic() {
+
+  }
+
+  @Override
+  public void testInit() {
+    // Cancels all running commands at the start of test mode.
+    CommandScheduler.getInstance().cancelAll();
+  }
+
+  /**
+   * This function is called periodically during test mode.
+   */
+  @Override
+  public void testPeriodic() {
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/RobotContainer.java

@@ -0,0 +1,100 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.gyrodrivecommands;
+
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.PIDCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+
+import edu.wpi.first.wpilibj.examples.gyrodrivecommands.commands.TurnToAngle;
+import edu.wpi.first.wpilibj.examples.gyrodrivecommands.subsystems.DriveSubsystem;
+
+import static edu.wpi.first.wpilibj.XboxController.Button;
+import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kStabilizationD;
+import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kStabilizationI;
+import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kStabilizationP;
+import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.OIConstants.kDriverControllerPort;
+
+/**
+ * This class is where the bulk of the robot should be declared.  Since Command-based is a
+ * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
+ * periodic methods (other than the scheduler calls).  Instead, the structure of the robot
+ * (including subsystems, commands, and button mappings) should be declared here.
+ */
+public class RobotContainer {
+  // The robot's subsystems
+  private final DriveSubsystem m_robotDrive = new DriveSubsystem();
+
+  // The driver's controller
+  XboxController m_driverController = new XboxController(kDriverControllerPort);
+
+  /**
+   * The container for the robot.  Contains subsystems, OI devices, and commands.
+   */
+  public RobotContainer() {
+    // Configure the button bindings
+    configureButtonBindings();
+
+    // Configure default commands
+    // Set the default drive command to split-stick arcade drive
+    m_robotDrive.setDefaultCommand(
+        // A split-stick arcade command, with forward/backward controlled by the left
+        // hand, and turning controlled by the right.
+        new RunCommand(() -> m_robotDrive
+            .arcadeDrive(m_driverController.getY(GenericHID.Hand.kLeft),
+                m_driverController.getX(GenericHID.Hand.kRight)), m_robotDrive));
+
+  }
+
+  /**
+   * Use this method to define your button->command mappings.  Buttons can be created by
+   * instantiating a {@link GenericHID} or one of its subclasses ({@link
+   * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then calling passing it to a
+   * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+   */
+  private void configureButtonBindings() {
+    // Drive at half speed when the right bumper is held
+    new JoystickButton(m_driverController, Button.kBumperRight.value)
+        .whenPressed(() -> m_robotDrive.setMaxOutput(.5))
+        .whenReleased(() -> m_robotDrive.setMaxOutput(1));
+
+    // Stabilize robot to drive straight with gyro when left bumper is held
+    new JoystickButton(m_driverController, Button.kBumperLeft.value).whenHeld(
+        new PIDCommand(
+            new PIDController(kStabilizationP, kStabilizationI, kStabilizationD),
+            // Close the loop on the turn rate
+            m_robotDrive::getTurnRate,
+            // Setpoint is 0
+            0,
+            // Pipe the output to the turning controls
+            output -> m_robotDrive
+                .arcadeDrive(m_driverController.getY(GenericHID.Hand.kLeft), output),
+            // Require the robot drive
+            m_robotDrive));
+
+    // Turn to 90 degrees when the 'X' button is pressed, with a 5 second timeout
+    new JoystickButton(m_driverController, Button.kX.value)
+        .whenPressed(new TurnToAngle(90, m_robotDrive).withTimeout(5));
+  }
+
+
+  /**
+   * Use this to pass the autonomous command to the main {@link Robot} class.
+   *
+   * @return the command to run in autonomous
+   */
+  public Command getAutonomousCommand() {
+    // no auto
+    return new InstantCommand();
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngle.java

@@ -0,0 +1,54 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.gyrodrivecommands.commands;
+
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj2.command.PIDCommand;
+
+import edu.wpi.first.wpilibj.examples.gyrodrivecommands.subsystems.DriveSubsystem;
+
+import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kTurnD;
+import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kTurnI;
+import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kTurnP;
+import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kTurnRateToleranceDegPerS;
+import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kTurnToleranceDeg;
+
+/**
+ * A command that will turn the robot to the specified angle.
+ */
+public class TurnToAngle extends PIDCommand {
+  /**
+   * Turns to robot to the specified angle.
+   *
+   * @param targetAngleDegrees The angle to turn to
+   * @param drive              The drive subsystem to use
+   */
+  public TurnToAngle(double targetAngleDegrees, DriveSubsystem drive) {
+    super(new PIDController(kTurnP, kTurnI, kTurnD),
+        // Close loop on heading
+        drive::getHeading,
+        // Set reference to target
+        targetAngleDegrees,
+        // Pipe output to turn robot
+        output -> drive.arcadeDrive(0, output),
+        // Require the drive
+        drive);
+
+    // Set the controller to be continuous (because it is an angle controller)
+    getController().enableContinuousInput(-180, 180);
+    // Set the controller tolerance - the delta tolerance ensures the robot is stationary at the
+    // setpoint before it is considered as having reached the reference
+    getController().setAbsoluteTolerance(kTurnToleranceDeg, kTurnRateToleranceDegPerS);
+  }
+
+  @Override
+  public boolean isFinished() {
+    // End when the controller is at the reference.
+    return getController().atSetpoint();
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/subsystems/DriveSubsystem.java

@@ -0,0 +1,141 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.gyrodrivecommands.subsystems;
+
+import edu.wpi.first.wpilibj.ADXRS450_Gyro;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.PWMVictorSPX;
+import edu.wpi.first.wpilibj.SpeedControllerGroup;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.interfaces.Gyro;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kEncoderDistancePerPulse;
+import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kGyroReversed;
+import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kLeftEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kLeftEncoderReversed;
+import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kLeftMotor1Port;
+import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kLeftMotor2Port;
+import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kRightEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kRightEncoderReversed;
+import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kRightMotor1Port;
+import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kRightMotor2Port;
+
+public class DriveSubsystem extends SubsystemBase {
+  // The motors on the left side of the drive.
+  private final SpeedControllerGroup m_leftMotors =
+      new SpeedControllerGroup(new PWMVictorSPX(kLeftMotor1Port),
+          new PWMVictorSPX(kLeftMotor2Port));
+
+  // The motors on the right side of the drive.
+  private final SpeedControllerGroup m_rightMotors =
+      new SpeedControllerGroup(new PWMVictorSPX(kRightMotor1Port),
+          new PWMVictorSPX(kRightMotor2Port));
+
+  // The robot's drive
+  private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
+
+  // The left-side drive encoder
+  private final Encoder m_leftEncoder =
+      new Encoder(kLeftEncoderPorts[0], kLeftEncoderPorts[1], kLeftEncoderReversed);
+
+  // The right-side drive encoder
+  private final Encoder m_rightEncoder =
+      new Encoder(kRightEncoderPorts[0], kRightEncoderPorts[1], kRightEncoderReversed);
+
+  // The gyro sensor
+  private final Gyro m_gyro = new ADXRS450_Gyro();
+
+  /**
+   * Creates a new DriveSubsystem.
+   */
+  public DriveSubsystem() {
+    // Sets the distance per pulse for the encoders
+    m_leftEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
+    m_rightEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
+  }
+
+  /**
+   * Drives the robot using arcade controls.
+   *
+   * @param fwd the commanded forward movement
+   * @param rot the commanded rotation
+   */
+  public void arcadeDrive(double fwd, double rot) {
+    m_drive.arcadeDrive(fwd, rot);
+  }
+
+  /**
+   * Resets the drive encoders to currently read a position of 0.
+   */
+  public void resetEncoders() {
+    m_leftEncoder.reset();
+    m_rightEncoder.reset();
+  }
+
+  /**
+   * Gets the average distance of the two encoders.
+   *
+   * @return the average of the two encoder readings
+   */
+  public double getAverageEncoderDistance() {
+    return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.;
+  }
+
+  /**
+   * Gets the left drive encoder.
+   *
+   * @return the left drive encoder
+   */
+  public Encoder getLeftEncoder() {
+    return m_leftEncoder;
+  }
+
+  /**
+   * Gets the right drive encoder.
+   *
+   * @return the right drive encoder
+   */
+  public Encoder getRightEncoder() {
+    return m_rightEncoder;
+  }
+
+  /**
+   * Sets the max output of the drive.  Useful for scaling the drive to drive more slowly.
+   *
+   * @param maxOutput the maximum output to which the drive will be constrained
+   */
+  public void setMaxOutput(double maxOutput) {
+    m_drive.setMaxOutput(maxOutput);
+  }
+
+  /**
+   * Zeroes the heading of the robot.
+   */
+  public void zeroHeading() {
+    m_gyro.reset();
+  }
+
+  /**
+   * Returns the heading of the robot.
+   *
+   * @return the robot's heading in degrees, from 180 to 180
+   */
+  public double getHeading() {
+    return Math.IEEEremainder(m_gyro.getAngle(), 360) * (kGyroReversed ? -1. : 1.);
+  }
+
+  /**
+   * Returns the turn rate of the robot.
+   *
+   * @return The turn rate of the robot, in degrees per second
+   */
+  public double getTurnRate() {
+    return m_gyro.getRate() * (kGyroReversed ? -1. : 1.);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Constants.java

@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.hatchbotinlined;
+
+/**
+ * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
+ * constants.  This class should not be used for any other purpose.  All constants should be
+ * declared globally (i.e. public static).  Do not put anything functional in this class.
+ *
+ * <p>It is advised to statically import this class (or one of its inner classes) wherever the
+ * constants are needed, to reduce verbosity.
+ */
+public final class Constants {
+  public static final class DriveConstants {
+    public static final int kLeftMotor1Port = 0;
+    public static final int kLeftMotor2Port = 1;
+    public static final int kRightMotor1Port = 2;
+    public static final int kRightMotor2Port = 3;
+
+    public static final int[] kLeftEncoderPorts = new int[]{0, 1};
+    public static final int[] kRightEncoderPorts = new int[]{2, 3};
+    public static final boolean kLeftEncoderReversed = false;
+    public static final boolean kRightEncoderReversed = true;
+
+    public static final int kEncoderCPR = 1024;
+    public static final double kWheelDiameterInches = 6;
+    public static final double kEncoderDistancePerPulse =
+        // Assumes the encoders are directly mounted on the wheel shafts
+        (kWheelDiameterInches * Math.PI) / (double) kEncoderCPR;
+  }
+
+  public static final class HatchConstants {
+    public static final int kHatchSolenoidModule = 0;
+    public static final int[] kHatchSolenoidPorts = new int[]{0, 1};
+  }
+
+  public static final class AutoConstants {
+    public static final double kAutoDriveDistanceInches = 60;
+    public static final double kAutoBackupDistanceInches = 20;
+    public static final double kAutoDriveSpeed = .5;
+  }
+
+  public static final class OIConstants {
+    public static final int kDriverControllerPort = 1;
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Main.java

@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.hatchbotinlined;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {
+  }
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Robot.java

@@ -0,0 +1,113 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.hatchbotinlined;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
+/**
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the build.gradle file in the
+ * project.
+ */
+public class Robot extends TimedRobot {
+  private Command m_autonomousCommand;
+
+  private RobotContainer m_robotContainer;
+
+  /**
+   * This function is run when the robot is first started up and should be used for any
+   * initialization code.
+   */
+  @Override
+  public void robotInit() {
+    // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
+    // autonomous chooser on the dashboard.
+    m_robotContainer = new RobotContainer();
+  }
+
+  /**
+   * This function is called every robot packet, no matter the mode. Use this for items like
+   * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
+   *
+   * <p>This runs after the mode specific periodic functions, but before
+   * LiveWindow and SmartDashboard integrated updating.
+   */
+  @Override
+  public void robotPeriodic() {
+    // Runs the Scheduler.  This is responsible for polling buttons, adding newly-scheduled
+    // commands, running already-scheduled commands, removing finished or interrupted commands,
+    // and running subsystem periodic() methods.  This must be called from the robot's periodic
+    // block in order for anything in the Command-based framework to work.
+    CommandScheduler.getInstance().run();
+  }
+
+  /**
+   * This function is called once each time the robot enters Disabled mode.
+   */
+  @Override
+  public void disabledInit() {
+  }
+
+  @Override
+  public void disabledPeriodic() {
+  }
+
+  /**
+   * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
+   */
+  @Override
+  public void autonomousInit() {
+    m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+
+    // schedule the autonomous command (example)
+    if (m_autonomousCommand != null) {
+      m_autonomousCommand.schedule();
+    }
+  }
+
+  /**
+   * This function is called periodically during autonomous.
+   */
+  @Override
+  public void autonomousPeriodic() {
+  }
+
+  @Override
+  public void teleopInit() {
+    // This makes sure that the autonomous stops running when
+    // teleop starts running. If you want the autonomous to
+    // continue until interrupted by another command, remove
+    // this line or comment it out.
+    if (m_autonomousCommand != null) {
+      m_autonomousCommand.cancel();
+    }
+  }
+
+  /**
+   * This function is called periodically during operator control.
+   */
+  @Override
+  public void teleopPeriodic() {
+  }
+
+  @Override
+  public void testInit() {
+    // Cancels all running commands at the start of test mode.
+    CommandScheduler.getInstance().cancelAll();
+  }
+
+  /**
+   * This function is called periodically during test mode.
+   */
+  @Override
+  public void testPeriodic() {
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/RobotContainer.java

@@ -0,0 +1,120 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.hatchbotinlined;
+
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.StartEndCommand;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+
+import edu.wpi.first.wpilibj.examples.hatchbotinlined.commands.ComplexAutoCommand;
+import edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems.DriveSubsystem;
+import edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems.HatchSubsystem;
+
+import static edu.wpi.first.wpilibj.XboxController.Button;
+import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.AutoConstants.kAutoDriveDistanceInches;
+import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.AutoConstants.kAutoDriveSpeed;
+import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.OIConstants.kDriverControllerPort;
+
+/**
+ * This class is where the bulk of the robot should be declared.  Since Command-based is a
+ * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
+ * periodic methods (other than the scheduler calls).  Instead, the structure of the robot
+ * (including subsystems, commands, and button mappings) should be declared here.
+ */
+public class RobotContainer {
+  // The robot's subsystems
+  private final DriveSubsystem m_robotDrive = new DriveSubsystem();
+  private final HatchSubsystem m_hatchSubsystem = new HatchSubsystem();
+
+  // The autonomous routines
+
+  // A simple auto routine that drives forward a specified distance, and then stops.
+  private final Command m_simpleAuto =
+      new StartEndCommand(
+          // Start driving forward at the start of the command
+          () -> m_robotDrive.arcadeDrive(kAutoDriveSpeed, 0),
+          // Stop driving at the end of the command
+          () -> m_robotDrive.arcadeDrive(0, 0),
+          // Requires the drive subsystem
+          m_robotDrive
+      )
+          // Reset the encoders before starting
+          .beforeStarting(m_robotDrive::resetEncoders)
+          // End the command when the robot's driven distance exceeds the desired value
+          .interruptOn(() -> m_robotDrive.getAverageEncoderDistance() >= kAutoDriveDistanceInches);
+
+  // A complex auto routine that drives forward, drops a hatch, and then drives backward.
+  private final Command m_complexAuto = new ComplexAutoCommand(m_robotDrive, m_hatchSubsystem);
+
+  // A chooser for autonomous commands
+  SendableChooser<Command> m_chooser = new SendableChooser<>();
+
+  // The driver's controller
+  XboxController m_driverController = new XboxController(kDriverControllerPort);
+
+  /**
+   * The container for the robot.  Contains subsystems, OI devices, and commands.
+   */
+  public RobotContainer() {
+    // Configure the button bindings
+    configureButtonBindings();
+
+    // Configure default commands
+    // Set the default drive command to split-stick arcade drive
+    m_robotDrive.setDefaultCommand(
+        // A split-stick arcade command, with forward/backward controlled by the left
+        // hand, and turning controlled by the right.
+        new RunCommand(() -> m_robotDrive.arcadeDrive(
+            m_driverController.getY(GenericHID.Hand.kLeft),
+            m_driverController.getX(GenericHID.Hand.kRight)),
+            m_robotDrive)
+    );
+
+    // Add commands to the autonomous command chooser
+    m_chooser.addOption("Simple Auto", m_simpleAuto);
+    m_chooser.addOption("Complex Auto", m_complexAuto);
+
+    // Put the chooser on the dashboard
+    Shuffleboard.getTab("Autonomous").add(m_chooser);
+  }
+
+  /**
+   * Use this method to define your button->command mappings.  Buttons can be created by
+   * instantiating a {@link GenericHID} or one of its subclasses ({@link
+   * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then calling passing it to a
+   * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+   */
+  private void configureButtonBindings() {
+    // Grab the hatch when the 'A' button is pressed.
+    new JoystickButton(m_driverController, Button.kA.value)
+        .whenPressed(new InstantCommand(m_hatchSubsystem::grabHatch, m_hatchSubsystem));
+    // Release the hatch when the 'B' button is pressed.
+    new JoystickButton(m_driverController, Button.kB.value)
+        .whenPressed(new InstantCommand(m_hatchSubsystem::releaseHatch, m_hatchSubsystem));
+    // While holding the shoulder button, drive at half speed
+    new JoystickButton(m_driverController, Button.kBumperRight.value)
+        .whenPressed(() -> m_robotDrive.setMaxOutput(.5))
+        .whenReleased(() -> m_robotDrive.setMaxOutput(1));
+  }
+
+
+  /**
+   * Use this to pass the autonomous command to the main {@link Robot} class.
+   *
+   * @return the command to run in autonomous
+   */
+  public Command getAutonomousCommand() {
+    return m_chooser.getSelected();
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/commands/ComplexAutoCommand.java

@@ -0,0 +1,61 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.hatchbotinlined.commands;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.StartEndCommand;
+
+import edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems.DriveSubsystem;
+import edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems.HatchSubsystem;
+
+import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.AutoConstants.kAutoBackupDistanceInches;
+import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.AutoConstants.kAutoDriveDistanceInches;
+import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.AutoConstants.kAutoDriveSpeed;
+
+/**
+ * A complex auto command that drives forward, releases a hatch, and then drives backward.
+ */
+public class ComplexAutoCommand extends SequentialCommandGroup {
+  /**
+   * Creates a new ComplexAutoCommand.
+   *
+   * @param driveSubsystem The drive subsystem this command will run on
+   * @param hatchSubsystem The hatch subsystem this command will run on
+   */
+  public ComplexAutoCommand(DriveSubsystem driveSubsystem, HatchSubsystem hatchSubsystem) {
+    addCommands(
+        // Drive forward up to the front of the cargo ship
+        new StartEndCommand(
+            // Start driving forward at the start of the command
+            () -> driveSubsystem.arcadeDrive(kAutoDriveSpeed, 0),
+            // Stop driving at the end of the command
+            () -> driveSubsystem.arcadeDrive(0, 0), driveSubsystem
+        )
+            // Reset the encoders before starting
+            .beforeStarting(driveSubsystem::resetEncoders)
+            // End the command when the robot's driven distance exceeds the desired value
+            .interruptOn(
+                () -> driveSubsystem.getAverageEncoderDistance() >= kAutoDriveDistanceInches),
+
+        // Release the hatch
+        new InstantCommand(hatchSubsystem::releaseHatch, hatchSubsystem),
+
+        // Drive backward the specified distance
+        new StartEndCommand(
+            () -> driveSubsystem.arcadeDrive(-kAutoDriveSpeed, 0),
+            () -> driveSubsystem.arcadeDrive(0, 0),
+            driveSubsystem
+        )
+            .beforeStarting(driveSubsystem::resetEncoders)
+            .interruptOn(
+                () -> driveSubsystem.getAverageEncoderDistance() <= -kAutoBackupDistanceInches)
+    );
+  }
+
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/DriveSubsystem.java

@@ -0,0 +1,110 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems;
+
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.PWMVictorSPX;
+import edu.wpi.first.wpilibj.SpeedControllerGroup;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants.kEncoderDistancePerPulse;
+import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants.kLeftEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants.kLeftEncoderReversed;
+import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants.kLeftMotor1Port;
+import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants.kLeftMotor2Port;
+import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants.kRightEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants.kRightEncoderReversed;
+import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants.kRightMotor1Port;
+import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants.kRightMotor2Port;
+
+public class DriveSubsystem extends SubsystemBase {
+  // The motors on the left side of the drive.
+  private final SpeedControllerGroup m_leftMotors =
+      new SpeedControllerGroup(new PWMVictorSPX(kLeftMotor1Port),
+          new PWMVictorSPX(kLeftMotor2Port));
+
+  // The motors on the right side of the drive.
+  private final SpeedControllerGroup m_rightMotors =
+      new SpeedControllerGroup(new PWMVictorSPX(kRightMotor1Port),
+          new PWMVictorSPX(kRightMotor2Port));
+
+  // The robot's drive
+  private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
+
+  // The left-side drive encoder
+  private final Encoder m_leftEncoder =
+      new Encoder(kLeftEncoderPorts[0], kLeftEncoderPorts[1], kLeftEncoderReversed);
+
+  // The right-side drive encoder
+  private final Encoder m_rightEncoder =
+      new Encoder(kRightEncoderPorts[0], kRightEncoderPorts[1], kRightEncoderReversed);
+
+  /**
+   * Creates a new DriveSubsystem.
+   */
+  public DriveSubsystem() {
+    // Sets the distance per pulse for the encoders
+    m_leftEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
+    m_rightEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
+  }
+
+  /**
+   * Drives the robot using arcade controls.
+   *
+   * @param fwd the commanded forward movement
+   * @param rot the commanded rotation
+   */
+  public void arcadeDrive(double fwd, double rot) {
+    m_drive.arcadeDrive(fwd, rot);
+  }
+
+  /**
+   * Resets the drive encoders to currently read a position of 0.
+   */
+  public void resetEncoders() {
+    m_leftEncoder.reset();
+    m_rightEncoder.reset();
+  }
+
+  /**
+   * Gets the average distance of the TWO encoders.
+   *
+   * @return the average of the TWO encoder readings
+   */
+  public double getAverageEncoderDistance() {
+    return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.;
+  }
+
+  /**
+   * Gets the left drive encoder.
+   *
+   * @return the left drive encoder
+   */
+  public Encoder getLeftEncoder() {
+    return m_leftEncoder;
+  }
+
+  /**
+   * Gets the right drive encoder.
+   *
+   * @return the right drive encoder
+   */
+  public Encoder getRightEncoder() {
+    return m_rightEncoder;
+  }
+
+  /**
+   * Sets the max output of the drive.  Useful for scaling the drive to drive more slowly.
+   *
+   * @param maxOutput the maximum output to which the drive will be constrained
+   */
+  public void setMaxOutput(double maxOutput) {
+    m_drive.setMaxOutput(maxOutput);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/HatchSubsystem.java

@@ -0,0 +1,38 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems;
+
+import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kForward;
+import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kReverse;
+import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.HatchConstants.kHatchSolenoidModule;
+import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.HatchConstants.kHatchSolenoidPorts;
+
+/**
+ * A hatch mechanism actuated by a single {@link edu.wpi.first.wpilibj.DoubleSolenoid}.
+ */
+public class HatchSubsystem extends SubsystemBase {
+  private final DoubleSolenoid m_hatchSolenoid =
+      new DoubleSolenoid(kHatchSolenoidModule, kHatchSolenoidPorts[0], kHatchSolenoidPorts[1]);
+
+  /**
+   * Grabs the hatch.
+   */
+  public void grabHatch() {
+    m_hatchSolenoid.set(kForward);
+  }
+
+  /**
+   * Releases the hatch.
+   */
+  public void releaseHatch() {
+    m_hatchSolenoid.set(kReverse);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Constants.java

@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.hatchbottraditional;
+
+/**
+ * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
+ * constants.  This class should not be used for any other purpose.  All constants should be
+ * declared globally (i.e. public static).  Do not put anything functional in this class.
+ *
+ * <p>It is advised to statically import this class (or one of its inner classes) wherever the
+ * constants are needed, to reduce verbosity.
+ */
+public final class Constants {
+  public static final class DriveConstants {
+    public static final int kLeftMotor1Port = 0;
+    public static final int kLeftMotor2Port = 1;
+    public static final int kRightMotor1Port = 2;
+    public static final int kRightMotor2Port = 3;
+
+    public static final int[] kLeftEncoderPorts = new int[]{0, 1};
+    public static final int[] kRightEncoderPorts = new int[]{2, 3};
+    public static final boolean kLeftEncoderReversed = false;
+    public static final boolean kRightEncoderReversed = true;
+
+    public static final int kEncoderCPR = 1024;
+    public static final double kWheelDiameterInches = 6;
+    public static final double kEncoderDistancePerPulse =
+        // Assumes the encoders are directly mounted on the wheel shafts
+        (kWheelDiameterInches * Math.PI) / (double) kEncoderCPR;
+  }
+
+  public static final class HatchConstants {
+    public static final int kHatchSolenoidModule = 0;
+    public static final int[] kHatchSolenoidPorts = new int[]{0, 1};
+  }
+
+  public static final class AutoConstants {
+    public static final double kAutoDriveDistanceInches = 60;
+    public static final double kAutoBackupDistanceInches = 20;
+    public static final double kAutoDriveSpeed = .5;
+  }
+
+  public static final class OIConstants {
+    public static final int kDriverControllerPort = 1;
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Main.java

@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.hatchbottraditional;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {
+  }
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Robot.java

@@ -0,0 +1,120 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.hatchbottraditional;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
+/**
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the build.gradle file in the
+ * project.
+ */
+public class Robot extends TimedRobot {
+  private Command m_autonomousCommand;
+
+  private RobotContainer m_robotContainer;
+
+  /**
+   * This function is run when the robot is first started up and should be used for any
+   * initialization code.
+   */
+  @Override
+  public void robotInit() {
+    // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
+    // autonomous chooser on the dashboard.
+    m_robotContainer = new RobotContainer();
+  }
+
+  /**
+   * This function is called every robot packet, no matter the mode. Use this for items like
+   * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
+   *
+   * <p>This runs after the mode specific periodic functions, but before
+   * LiveWindow and SmartDashboard integrated updating.
+   */
+  @Override
+  public void robotPeriodic() {
+    // Runs the Scheduler.  This is responsible for polling buttons, adding newly-scheduled
+    // commands, running already-scheduled commands, removing finished or interrupted commands,
+    // and running subsystem periodic() methods.  This must be called from the robot's periodic
+    // block in order for anything in the Command-based framework to work.
+    CommandScheduler.getInstance().run();
+  }
+
+  /**
+   * This function is called once each time the robot enters Disabled mode.
+   */
+  @Override
+  public void disabledInit() {
+  }
+
+  @Override
+  public void disabledPeriodic() {
+  }
+
+  /**
+   * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
+   */
+  @Override
+  public void autonomousInit() {
+    m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+
+    /*
+     * String autoSelected = SmartDashboard.getString("Auto Selector",
+     * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
+     * = new MyAutoCommand(); break; case "Default Auto": default:
+     * autonomousCommand = new ExampleCommand(); break; }
+     */
+
+    // schedule the autonomous command (example)
+    if (m_autonomousCommand != null) {
+      m_autonomousCommand.schedule();
+    }
+  }
+
+  /**
+   * This function is called periodically during autonomous.
+   */
+  @Override
+  public void autonomousPeriodic() {
+  }
+
+  @Override
+  public void teleopInit() {
+    // This makes sure that the autonomous stops running when
+    // teleop starts running. If you want the autonomous to
+    // continue until interrupted by another command, remove
+    // this line or comment it out.
+    if (m_autonomousCommand != null) {
+      m_autonomousCommand.cancel();
+    }
+  }
+
+  /**
+   * This function is called periodically during operator control.
+   */
+  @Override
+  public void teleopPeriodic() {
+  }
+
+  @Override
+  public void testInit() {
+    // Cancels all running commands at the start of test mode.
+    CommandScheduler.getInstance().cancelAll();
+  }
+
+  /**
+   * This function is called periodically during test mode.
+   */
+  @Override
+  public void testPeriodic() {
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/RobotContainer.java

@@ -0,0 +1,110 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.hatchbottraditional;
+
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+
+import edu.wpi.first.wpilibj.examples.hatchbottraditional.commands.ComplexAuto;
+import edu.wpi.first.wpilibj.examples.hatchbottraditional.commands.DefaultDrive;
+import edu.wpi.first.wpilibj.examples.hatchbottraditional.commands.DriveDistance;
+import edu.wpi.first.wpilibj.examples.hatchbottraditional.commands.GrabHatch;
+import edu.wpi.first.wpilibj.examples.hatchbottraditional.commands.HalveDriveSpeed;
+import edu.wpi.first.wpilibj.examples.hatchbottraditional.commands.ReleaseHatch;
+import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.DriveSubsystem;
+import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.HatchSubsystem;
+
+import static edu.wpi.first.wpilibj.XboxController.Button;
+import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.AutoConstants.kAutoDriveDistanceInches;
+import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.AutoConstants.kAutoDriveSpeed;
+import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.OIConstants.kDriverControllerPort;
+
+/**
+ * This class is where the bulk of the robot should be declared.  Since Command-based is a
+ * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
+ * periodic methods (other than the scheduler calls).  Instead, the structure of the robot
+ * (including subsystems, commands, and button mappings) should be declared here.
+ */
+public class RobotContainer {
+  // The robot's subsystems
+  private final DriveSubsystem m_robotDrive = new DriveSubsystem();
+  private final HatchSubsystem m_hatchSubsystem = new HatchSubsystem();
+
+  // The autonomous routines
+
+  // A simple auto routine that drives forward a specified distance, and then stops.
+  private final Command m_simpleAuto =
+      new DriveDistance(kAutoDriveDistanceInches, kAutoDriveSpeed, m_robotDrive);
+
+  // A complex auto routine that drives forward, drops a hatch, and then drives backward.
+  private final Command m_complexAuto = new ComplexAuto(m_robotDrive, m_hatchSubsystem);
+
+  // A chooser for autonomous commands
+  SendableChooser<Command> m_chooser = new SendableChooser<>();
+
+  // The driver's controller
+  XboxController m_driverController = new XboxController(kDriverControllerPort);
+
+  /**
+   * The container for the robot.  Contains subsystems, OI devices, and commands.
+   */
+  public RobotContainer() {
+    // Configure the button bindings
+    configureButtonBindings();
+
+    // Configure default commands
+    // Set the default drive command to split-stick arcade drive
+    m_robotDrive.setDefaultCommand(
+        // A split-stick arcade command, with forward/backward controlled by the left
+        // hand, and turning controlled by the right.
+        new DefaultDrive(
+            m_robotDrive,
+            () -> m_driverController.getY(GenericHID.Hand.kLeft),
+            () -> m_driverController.getX(GenericHID.Hand.kRight))
+    );
+
+    // Add commands to the autonomous command chooser
+    m_chooser.addOption("Simple Auto", m_simpleAuto);
+    m_chooser.addOption("Complex Auto", m_complexAuto);
+
+    // Put the chooser on the dashboard
+    Shuffleboard.getTab("Autonomous").add(m_chooser);
+  }
+
+  /**
+   * Use this method to define your button->command mappings.  Buttons can be created by
+   * instantiating a {@link GenericHID} or one of its subclasses ({@link
+   * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then calling passing it to a
+   * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+   */
+  private void configureButtonBindings() {
+    // Grab the hatch when the 'A' button is pressed.
+    new JoystickButton(m_driverController, Button.kA.value)
+        .whenPressed(new GrabHatch(m_hatchSubsystem));
+    // Release the hatch when the 'B' button is pressed.
+    new JoystickButton(m_driverController, Button.kB.value)
+        .whenPressed(new ReleaseHatch(m_hatchSubsystem));
+    // While holding the shoulder button, drive at half speed
+    new JoystickButton(m_driverController, Button.kBumperRight.value)
+        .whenHeld(new HalveDriveSpeed(m_robotDrive));
+  }
+
+
+  /**
+   * Use this to pass the autonomous command to the main {@link Robot} class.
+   *
+   * @return the command to run in autonomous
+   */
+  public Command getAutonomousCommand() {
+    return m_chooser.getSelected();
+  }
+}