Split the two command implementations into separate libraries (#2012)

This will allow us at the user code side to determine to include old commands, new commands or both.

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

@@ -0,0 +1,309 @@
+/*----------------------------------------------------------------------------*/
+/* 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 ParallelRaceGroup 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 ParallelRaceGroup withInterrupt(BooleanSupplier condition) {
+    return new ParallelRaceGroup(this, new WaitUntilCommand(condition));
+  }
+
+  /**
+   * 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 SequentialCommandGroup beforeStarting(Runnable toRun) {
+    return new SequentialCommandGroup(new InstantCommand(toRun), this);
+  }
+
+  /**
+   * 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 SequentialCommandGroup andThen(Runnable toRun) {
+    return new SequentialCommandGroup(this, new InstantCommand(toRun));
+  }
+
+  /**
+   * 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 SequentialCommandGroup 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 ParallelDeadlineGroup 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 ParallelCommandGroup 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 ParallelRaceGroup 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 PerpetualCommand 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 ProxyScheduleCommand 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;
+  }
+
+  /**
+   * Gets the name of this Command.
+   *
+   * @return Name
+   */
+  default String getName() {
+    return this.getClass().getSimpleName();
+  }
+}

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

@@ -0,0 +1,102 @@
+/*----------------------------------------------------------------------------*/
+/* 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;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * A {@link Sendable} base class for {@link Command}s.
+ */
+@SuppressWarnings("PMD.AbstractClassWithoutAbstractMethod")
+public abstract class CommandBase implements Sendable, Command {
+
+  protected Set<Subsystem> m_requirements = new HashSet<>();
+
+  protected CommandBase() {
+    String name = getClass().getName();
+    SendableRegistry.add(this, name.substring(name.lastIndexOf('.') + 1));
+  }
+
+  /**
+   * 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 SendableRegistry.getName(this);
+  }
+
+  /**
+   * Sets the name of this Command.
+   *
+   * @param name name
+   */
+  @Override
+  public void setName(String name) {
+    SendableRegistry.setName(this, name);
+  }
+
+  /**
+   * Gets the subsystem name of this Command.
+   *
+   * @return Subsystem name
+   */
+  @Override
+  public String getSubsystem() {
+    return SendableRegistry.getSubsystem(this);
+  }
+
+  /**
+   * Sets the subsystem name of this Command.
+   *
+   * @param subsystem subsystem name
+   */
+  @Override
+  public void setSubsystem(String subsystem) {
+    SendableRegistry.setSubsystem(this, 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);
+  }
+}

wpilibNewCommands/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);
+  }
+}

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

@@ -0,0 +1,509 @@
+/*----------------------------------------------------------------------------*/
+/* 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.Sendable;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * 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 implements Sendable {
+  /**
+   * 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<>();
+
+  // Flag and queues for avoiding ConcurrentModificationException if commands are
+  // scheduled/canceled during run
+  private boolean m_inRunLoop;
+  private final Map<Command, Boolean> m_toSchedule = new LinkedHashMap<>();
+  private final List<Command> m_toCancel = new ArrayList<>();
+
+
+  CommandScheduler() {
+    HAL.report(tResourceType.kResourceType_Command, tInstances.kCommand_Scheduler);
+    SendableRegistry.addLW(this, "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 (m_inRunLoop) {
+      m_toSchedule.put(command, interruptible);
+      return;
+    }
+
+    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();
+    }
+
+    m_inRunLoop = true;
+    //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());
+      }
+    }
+    m_inRunLoop = false;
+
+    //Schedule/cancel commands from queues populated during loop
+    for (Map.Entry<Command, Boolean> commandInterruptible : m_toSchedule.entrySet()) {
+      schedule(commandInterruptible.getValue(), commandInterruptible.getKey());
+    }
+
+    for (Command command : m_toCancel) {
+      cancel(command);
+    }
+
+    m_toSchedule.clear();
+    m_toCancel.clear();
+
+    //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) {
+    if (m_inRunLoop) {
+      m_toCancel.addAll(List.of(commands));
+      return;
+    }
+
+    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]));
+    });
+  }
+}

wpilibNewCommands/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;
+  }
+}

wpilibNewCommands/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();
+  }
+}

wpilibNewCommands/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();
+  }
+}

wpilibNewCommands/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;
+  }
+}

wpilibNewCommands/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#withInterrupt(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();
+  }
+}

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

@@ -0,0 +1,92 @@
+/*----------------------------------------------------------------------------*/
+/* 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() {
+    m_useOutput.accept(m_controller.calculate(m_measurement.getAsDouble(),
+                                              m_setpoint.getAsDouble()));
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    m_useOutput.accept(0);
+  }
+
+  /**
+   * Returns the PIDController used by the command.
+   *
+   * @return The PIDController
+   */
+  public PIDController getController() {
+    return m_controller;
+  }
+}

wpilibNewCommands/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);
+  }
+}

wpilibNewCommands/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;
+  }
+}

wpilibNewCommands/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;
+  }
+}

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroup.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.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;
+      }
+    }
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    for (Command command : m_commands) {
+      command.end(!command.isFinished());
+    }
+  }
+
+  @Override
+  public boolean isFinished() {
+    return m_finished;
+  }
+
+  @Override
+  public boolean runsWhenDisabled() {
+    return m_runWhenDisabled;
+  }
+}

wpilibNewCommands/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();
+  }
+}

wpilibNewCommands/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;
+  }
+}

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDCommand.java

@@ -0,0 +1,138 @@
+/*----------------------------------------------------------------------------*/
+/* 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.BiConsumer;
+import java.util.function.DoubleSupplier;
+import java.util.function.Supplier;
+
+import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
+
+import static edu.wpi.first.wpilibj.trajectory.TrapezoidProfile.State;
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * A command that controls an output with a {@link ProfiledPIDController}.  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 ProfiledPIDCommand extends CommandBase {
+  protected final ProfiledPIDController m_controller;
+  protected DoubleSupplier m_measurement;
+  protected Supplier<State> m_goal;
+  protected BiConsumer<Double, State> m_useOutput;
+
+  /**
+   * Creates a new PIDCommand, which controls the given output with a ProfiledPIDController.
+   * Goal velocity is specified.
+   *
+   * @param controller        the controller that controls the output.
+   * @param measurementSource the measurement of the process variable
+   * @param goalSource        the controller's goal
+   * @param useOutput         the controller's output
+   * @param requirements      the subsystems required by this command
+   */
+  public ProfiledPIDCommand(ProfiledPIDController controller, DoubleSupplier measurementSource,
+                            Supplier<State> goalSource, BiConsumer<Double, State> useOutput,
+                            Subsystem... requirements) {
+    requireNonNullParam(controller, "controller", "SynchronousPIDCommand");
+    requireNonNullParam(measurementSource, "measurementSource", "SynchronousPIDCommand");
+    requireNonNullParam(goalSource, "goalSource", "SynchronousPIDCommand");
+    requireNonNullParam(useOutput, "useOutput", "SynchronousPIDCommand");
+
+    m_controller = controller;
+    m_useOutput = useOutput;
+    m_measurement = measurementSource;
+    m_goal = goalSource;
+    m_requirements.addAll(Set.of(requirements));
+  }
+
+  /**
+   * Creates a new PIDCommand, which controls the given output with a ProfiledPIDController.
+   * Goal velocity is implicitly zero.
+   *
+   * @param controller        the controller that controls the output.
+   * @param measurementSource the measurement of the process variable
+   * @param goalSource        the controller's goal
+   * @param useOutput         the controller's output
+   * @param requirements      the subsystems required by this command
+   */
+  public ProfiledPIDCommand(ProfiledPIDController controller, DoubleSupplier measurementSource,
+                            DoubleSupplier goalSource, BiConsumer<Double, State> useOutput,
+                            Subsystem... requirements) {
+    requireNonNullParam(controller, "controller", "SynchronousPIDCommand");
+    requireNonNullParam(measurementSource, "measurementSource", "SynchronousPIDCommand");
+    requireNonNullParam(goalSource, "goalSource", "SynchronousPIDCommand");
+    requireNonNullParam(useOutput, "useOutput", "SynchronousPIDCommand");
+
+    m_controller = controller;
+    m_useOutput = useOutput;
+    m_measurement = measurementSource;
+    m_goal = () -> new State(goalSource.getAsDouble(), 0);
+    m_requirements.addAll(Set.of(requirements));
+  }
+
+  /**
+   * Creates a new PIDCommand, which controls the given output with a ProfiledPIDController. Goal
+   * velocity is specified.
+   *
+   * @param controller        the controller that controls the output.
+   * @param measurementSource the measurement of the process variable
+   * @param goal              the controller's goal
+   * @param useOutput         the controller's output
+   * @param requirements      the subsystems required by this command
+   */
+  public ProfiledPIDCommand(ProfiledPIDController controller, DoubleSupplier measurementSource,
+                            State goal, BiConsumer<Double, State> useOutput,
+                            Subsystem... requirements) {
+    this(controller, measurementSource, () -> goal, useOutput, requirements);
+  }
+
+  /**
+   * Creates a new PIDCommand, which controls the given output with a ProfiledPIDController. Goal
+   * velocity is implicitly zero.
+   *
+   * @param controller        the controller that controls the output.
+   * @param measurementSource the measurement of the process variable
+   * @param goal              the controller's goal
+   * @param useOutput         the controller's output
+   * @param requirements      the subsystems required by this command
+   */
+  public ProfiledPIDCommand(ProfiledPIDController controller, DoubleSupplier measurementSource,
+                            double goal, BiConsumer<Double, State> useOutput,
+                            Subsystem... requirements) {
+    this(controller, measurementSource, () -> goal, useOutput, requirements);
+  }
+
+  @Override
+  public void initialize() {
+    m_controller.reset();
+  }
+
+  @Override
+  public void execute() {
+    m_useOutput.accept(m_controller.calculate(m_measurement.getAsDouble(), m_goal.get()),
+                       m_controller.getSetpoint());
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    m_useOutput.accept(0., new State());
+  }
+
+  /**
+   * Returns the ProfiledPIDController used by the command.
+   *
+   * @return The ProfiledPIDController
+   */
+  public ProfiledPIDController getController() {
+    return m_controller;
+  }
+}

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDSubsystem.java

@@ -0,0 +1,81 @@
+/*----------------------------------------------------------------------------*/
+/* 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.ProfiledPIDController;
+
+import static edu.wpi.first.wpilibj.trajectory.TrapezoidProfile.State;
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * A subsystem that uses a {@link ProfiledPIDController} to control an output.  The controller is
+ * run synchronously from the subsystem's periodic() method.
+ */
+public abstract class ProfiledPIDSubsystem extends SubsystemBase {
+  protected final ProfiledPIDController m_controller;
+  protected boolean m_enabled;
+
+  /**
+   * Creates a new ProfiledPIDSubsystem.
+   *
+   * @param controller the ProfiledPIDController to use
+   */
+  public ProfiledPIDSubsystem(ProfiledPIDController controller) {
+    requireNonNullParam(controller, "controller", "ProfiledPIDSubsystem");
+    m_controller = controller;
+  }
+
+  @Override
+  public void periodic() {
+    if (m_enabled) {
+      useOutput(m_controller.calculate(getMeasurement(), getGoal()), m_controller.getSetpoint());
+    }
+  }
+
+  public ProfiledPIDController getController() {
+    return m_controller;
+  }
+
+  /**
+   * Uses the output from the ProfiledPIDController.
+   *
+   * @param output the output of the ProfiledPIDController
+   * @param goal   the goal state of the ProfiledPIDController, for feedforward
+   */
+  public abstract void useOutput(double output, State goal);
+
+  /**
+   * Returns the goal used by the ProfiledPIDController.
+   *
+   * @return the goal to be used by the controller
+   */
+  public abstract State getGoal();
+
+  /**
+   * Returns the measurement of the process variable used by the ProfiledPIDController.
+   *
+   * @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, new State());
+  }
+}

wpilibNewCommands/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;
+  }
+}

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java

@@ -0,0 +1,231 @@
+/*----------------------------------------------------------------------------*/
+/* 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.function.BiConsumer;
+import java.util.function.DoubleSupplier;
+import java.util.function.Supplier;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.controller.RamseteController;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
+import edu.wpi.first.wpilibj.trajectory.Trajectory;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * A command that uses a RAMSETE controller ({@link RamseteController}) to follow a trajectory
+ * {@link Trajectory} with a differential drive.
+ *
+ * <p>The command handles trajectory-following, PID calculations, and feedforwards internally.  This
+ * is intended to be a more-or-less "complete solution" that can be used by teams without a great
+ * deal of controls expertise.
+ *
+ * <p>Advanced teams seeking more flexibility (for example, those who wish to use the onboard
+ * PID functionality of a "smart" motor controller) may use the secondary constructor that omits
+ * the PID and feedforward functionality, returning only the raw wheel speeds from the RAMSETE
+ * controller.
+ */
+@SuppressWarnings("PMD.TooManyFields")
+public class RamseteCommand extends CommandBase {
+  private final Timer m_timer = new Timer();
+  private DifferentialDriveWheelSpeeds m_prevSpeeds;
+  private double m_prevTime;
+  private final boolean m_usePID;
+
+  private final Trajectory m_trajectory;
+  private final Supplier<Pose2d> m_pose;
+  private final RamseteController m_follower;
+  private final double m_ks;
+  private final double m_kv;
+  private final double m_ka;
+  private final DifferentialDriveKinematics m_kinematics;
+  private final DoubleSupplier m_leftSpeed;
+  private final DoubleSupplier m_rightSpeed;
+  private final PIDController m_leftController;
+  private final PIDController m_rightController;
+  private final BiConsumer<Double, Double> m_output;
+
+  /**
+   * Constructs a new RamseteCommand that, when executed, will follow the provided trajectory.
+   * PID control and feedforward are handled internally, and outputs are scaled -1 to 1 for easy
+   * consumption by speed controllers.
+   *
+   * <p>Note: The controller will *not* set the outputVolts to zero upon completion of the path -
+   * this
+   * is left to the user, since it is not appropriate for paths with nonstationary endstates.
+   *
+   * @param trajectory                     The trajectory to follow.
+   * @param pose                           A function that supplies the robot pose - use one of
+   *                                       the odometry classes to provide this.
+   * @param controller                       The RAMSETE controller used to follow the trajectory.
+   * @param ksVolts                        Constant feedforward term for the robot drive.
+   * @param kvVoltSecondsPerMeter          Velocity-proportional feedforward term for the robot
+   *                                       drive.
+   * @param kaVoltSecondsSquaredPerMeter   Acceleration-proportional feedforward term for the robot
+   *                                       drive.
+   * @param kinematics                     The kinematics for the robot drivetrain.
+   * @param leftWheelSpeedMetersPerSecond  A function that supplies the speed of the left side of
+   *                                       the robot drive.
+   * @param rightWheelSpeedMetersPerSecond A function that supplies the speed of the right side of
+   *                                       the robot drive.
+   * @param leftController                 The PIDController for the left side of the robot drive.
+   * @param rightController                The PIDController for the right side of the robot drive.
+   * @param outputVolts                    A function that consumes the computed left and right
+   *                                       outputs (in volts) for the robot drive.
+   * @param requirements                   The subsystems to require.
+   */
+  @SuppressWarnings("PMD.ExcessiveParameterList")
+  public RamseteCommand(Trajectory trajectory,
+                        Supplier<Pose2d> pose,
+                        RamseteController controller,
+                        double ksVolts,
+                        double kvVoltSecondsPerMeter,
+                        double kaVoltSecondsSquaredPerMeter,
+                        DifferentialDriveKinematics kinematics,
+                        DoubleSupplier leftWheelSpeedMetersPerSecond,
+                        DoubleSupplier rightWheelSpeedMetersPerSecond,
+                        PIDController leftController,
+                        PIDController rightController,
+                        BiConsumer<Double, Double> outputVolts,
+                        Subsystem... requirements) {
+    m_trajectory = requireNonNullParam(trajectory, "trajectory", "RamseteCommand");
+    m_pose = requireNonNullParam(pose, "pose", "RamseteCommand");
+    m_follower = requireNonNullParam(controller, "controller", "RamseteCommand");
+    m_ks = ksVolts;
+    m_kv = kvVoltSecondsPerMeter;
+    m_ka = kaVoltSecondsSquaredPerMeter;
+    m_kinematics = requireNonNullParam(kinematics, "kinematics", "RamseteCommand");
+    m_leftSpeed = requireNonNullParam(leftWheelSpeedMetersPerSecond,
+                                      "leftWheelSpeedMetersPerSecond",
+                                      "RamseteCommand");
+    m_rightSpeed = requireNonNullParam(rightWheelSpeedMetersPerSecond,
+                                       "rightWheelSpeedMetersPerSecond",
+                                       "RamseteCommand");
+    m_leftController = requireNonNullParam(leftController, "leftController", "RamseteCommand");
+    m_rightController = requireNonNullParam(rightController, "rightController", "RamseteCommand");
+    m_output = requireNonNullParam(outputVolts, "outputVolts", "RamseteCommand");
+
+    m_usePID = true;
+
+    addRequirements(requirements);
+  }
+
+  /**
+   * Constructs a new RamseteCommand that, when executed, will follow the provided trajectory.
+   * Performs no PID control and calculates no feedforwards; outputs are the raw wheel speeds
+   * from the RAMSETE controller, and will need to be converted into a usable form by the user.
+   *
+   * @param trajectory            The trajectory to follow.
+   * @param pose                  A function that supplies the robot pose - use one of
+   *                              the odometry classes to provide this.
+   * @param follower              The RAMSETE follower used to follow the trajectory.
+   * @param kinematics            The kinematics for the robot drivetrain.
+   * @param outputMetersPerSecond A function that consumes the computed left and right
+   *                              wheel speeds.
+   * @param requirements          The subsystems to require.
+   */
+  public RamseteCommand(Trajectory trajectory,
+                        Supplier<Pose2d> pose,
+                        RamseteController follower,
+                        DifferentialDriveKinematics kinematics,
+                        BiConsumer<Double, Double> outputMetersPerSecond,
+                        Subsystem... requirements) {
+    m_trajectory = requireNonNullParam(trajectory, "trajectory", "RamseteCommand");
+    m_pose = requireNonNullParam(pose, "pose", "RamseteCommand");
+    m_follower = requireNonNullParam(follower, "follower", "RamseteCommand");
+    m_kinematics = requireNonNullParam(kinematics, "kinematics", "RamseteCommand");
+    m_output = requireNonNullParam(outputMetersPerSecond, "output", "RamseteCommand");
+
+    m_ks = 0;
+    m_kv = 0;
+    m_ka = 0;
+    m_leftSpeed = null;
+    m_rightSpeed = null;
+    m_leftController = null;
+    m_rightController = null;
+
+    m_usePID = false;
+
+    addRequirements(requirements);
+  }
+
+  @Override
+  public void initialize() {
+    m_prevTime = 0;
+    var initialState = m_trajectory.sample(0);
+    m_prevSpeeds = m_kinematics.toWheelSpeeds(
+        new ChassisSpeeds(initialState.velocityMetersPerSecond,
+                          0,
+                          initialState.curvatureRadPerMeter
+                              * initialState.velocityMetersPerSecond));
+    m_timer.reset();
+    m_timer.start();
+    if (m_usePID) {
+      m_leftController.reset();
+      m_rightController.reset();
+    }
+  }
+
+  @Override
+  public void execute() {
+    double curTime = m_timer.get();
+    double dt = curTime - m_prevTime;
+
+    var targetWheelSpeeds = m_kinematics.toWheelSpeeds(
+        m_follower.calculate(m_pose.get(), m_trajectory.sample(curTime)));
+
+    var leftSpeedSetpoint = targetWheelSpeeds.leftMetersPerSecond;
+    var rightSpeedSetpoint = targetWheelSpeeds.rightMetersPerSecond;
+
+    double leftOutput;
+    double rightOutput;
+
+    if (m_usePID) {
+      double leftFeedforward =
+          m_ks * Math.signum(leftSpeedSetpoint)
+              + m_kv * leftSpeedSetpoint
+              + m_ka * (leftSpeedSetpoint - m_prevSpeeds.leftMetersPerSecond) / dt;
+
+      double rightFeedforward =
+          m_ks * Math.signum(rightSpeedSetpoint)
+              + m_kv * rightSpeedSetpoint
+              + m_ka * (rightSpeedSetpoint - m_prevSpeeds.rightMetersPerSecond) / dt;
+
+      leftOutput = leftFeedforward
+          + m_leftController.calculate(m_leftSpeed.getAsDouble(),
+                                       leftSpeedSetpoint);
+
+      rightOutput = rightFeedforward
+          + m_rightController.calculate(m_rightSpeed.getAsDouble(),
+                                        rightSpeedSetpoint);
+    } else {
+      leftOutput = leftSpeedSetpoint;
+      rightOutput = rightSpeedSetpoint;
+    }
+
+    m_output.accept(leftOutput, rightOutput);
+
+    m_prevTime = curTime;
+    m_prevSpeeds = targetWheelSpeeds;
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    m_timer.stop();
+  }
+
+  @Override
+  public boolean isFinished() {
+    return m_timer.hasPeriodPassed(m_trajectory.getTotalTimeSeconds());
+  }
+}

wpilibNewCommands/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#withInterrupt(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();
+  }
+}

wpilibNewCommands/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;
+  }
+}

wpilibNewCommands/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();
+    }
+  }
+}

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

@@ -0,0 +1,97 @@
+/*----------------------------------------------------------------------------*/
+/* 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_currentCommandIndex > -1
+        && m_currentCommandIndex < m_commands.size()
+    ) {
+      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;
+  }
+}

wpilibNewCommands/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#withInterrupt(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();
+  }
+}

wpilibNewCommands/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);
+  }
+}

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

@@ -0,0 +1,93 @@
+/*----------------------------------------------------------------------------*/
+/* 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.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * 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 {
+
+  /**
+   * Constructor.
+   */
+  public SubsystemBase() {
+    String name = this.getClass().getSimpleName();
+    name = name.substring(name.lastIndexOf('.') + 1);
+    SendableRegistry.addLW(this, name, name);
+    CommandScheduler.getInstance().registerSubsystem(this);
+  }
+
+  /**
+   * Gets the name of this Subsystem.
+   *
+   * @return Name
+   */
+  @Override
+  public String getName() {
+    return SendableRegistry.getName(this);
+  }
+
+  /**
+   * Sets the name of this Subsystem.
+   *
+   * @param name name
+   */
+  @Override
+  public void setName(String name) {
+    SendableRegistry.setName(this, name);
+  }
+
+  /**
+   * Gets the subsystem name of this Subsystem.
+   *
+   * @return Subsystem name
+   */
+  @Override
+  public String getSubsystem() {
+    return SendableRegistry.getSubsystem(this);
+  }
+
+  /**
+   * Sets the subsystem name of this Subsystem.
+   *
+   * @param subsystem subsystem name
+   */
+  @Override
+  public void setSubsystem(String subsystem) {
+    SendableRegistry.setSubsystem(this, 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) {
+    SendableRegistry.addLW(child, getSubsystem(), name);
+    SendableRegistry.addChild(this, 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);
+  }
+}

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java

@@ -0,0 +1,64 @@
+/*----------------------------------------------------------------------------*/
+/* 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.function.Consumer;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+
+import static edu.wpi.first.wpilibj.trajectory.TrapezoidProfile.State;
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * A command that runs a {@link TrapezoidProfile}.  Useful for smoothly controlling mechanism
+ * motion.
+ */
+public class TrapezoidProfileCommand extends CommandBase {
+  private final TrapezoidProfile m_profile;
+  private final Consumer<State> m_output;
+
+  private final Timer m_timer = new Timer();
+
+  /**
+   * Creates a new TrapezoidProfileCommand that will execute the given {@link TrapezoidProfile}.
+   * Output will be piped to the provided consumer function.
+   *
+   * @param profile      The motion profile to execute.
+   * @param output       The consumer for the profile output.
+   * @param requirements The subsystems required by this command.
+   */
+  public TrapezoidProfileCommand(TrapezoidProfile profile,
+                                 Consumer<State> output,
+                                 Subsystem... requirements) {
+    m_profile = requireNonNullParam(profile, "profile", "TrapezoidProfileCommand");
+    m_output = requireNonNullParam(output, "output", "TrapezoidProfileCommand");
+    addRequirements(requirements);
+  }
+
+  @Override
+  public void initialize() {
+    m_timer.reset();
+    m_timer.start();
+  }
+
+  @Override
+  public void execute() {
+    m_output.accept(m_profile.calculate(m_timer.get()));
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    m_timer.stop();
+  }
+
+  @Override
+  public boolean isFinished() {
+    return m_timer.hasPeriodPassed(m_profile.totalTime());
+  }
+}

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WaitCommand.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.wpilibj2.command;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * 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;
+    SendableRegistry.setName(this, getName() + ": " + 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;
+  }
+}

wpilibNewCommands/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;
+  }
+}

wpilibNewCommands/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;
+  }
+}

wpilibNewCommands/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;
+  }
+}

wpilibNewCommands/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);
+  }
+}

wpilibNewCommands/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;
+  }
+}

wpilibNewCommands/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());
+  }
+}

wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp

@@ -0,0 +1,108 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/Command.h"
+
+#include <iostream>
+
+#include "frc2/command/CommandScheduler.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/ParallelCommandGroup.h"
+#include "frc2/command/ParallelDeadlineGroup.h"
+#include "frc2/command/ParallelRaceGroup.h"
+#include "frc2/command/PerpetualCommand.h"
+#include "frc2/command/ProxyScheduleCommand.h"
+#include "frc2/command/SequentialCommandGroup.h"
+#include "frc2/command/WaitCommand.h"
+#include "frc2/command/WaitUntilCommand.h"
+
+using namespace frc2;
+
+Command::~Command() { CommandScheduler::GetInstance().Cancel(this); }
+
+Command::Command(const Command& rhs) : ErrorBase(rhs) {}
+
+Command& Command::operator=(const Command& rhs) {
+  ErrorBase::operator=(rhs);
+  m_isGrouped = false;
+  return *this;
+}
+
+void Command::Initialize() {}
+void Command::Execute() {}
+void Command::End(bool interrupted) {}
+
+ParallelRaceGroup Command::WithTimeout(units::second_t duration) && {
+  std::vector<std::unique_ptr<Command>> temp;
+  temp.emplace_back(std::make_unique<WaitCommand>(duration));
+  temp.emplace_back(std::move(*this).TransferOwnership());
+  return ParallelRaceGroup(std::move(temp));
+}
+
+ParallelRaceGroup Command::WithInterrupt(std::function<bool()> condition) && {
+  std::vector<std::unique_ptr<Command>> temp;
+  temp.emplace_back(std::make_unique<WaitUntilCommand>(std::move(condition)));
+  temp.emplace_back(std::move(*this).TransferOwnership());
+  return ParallelRaceGroup(std::move(temp));
+}
+
+SequentialCommandGroup Command::BeforeStarting(std::function<void()> toRun) && {
+  std::vector<std::unique_ptr<Command>> temp;
+  temp.emplace_back(std::make_unique<InstantCommand>(
+      std::move(toRun), std::initializer_list<Subsystem*>{}));
+  temp.emplace_back(std::move(*this).TransferOwnership());
+  return SequentialCommandGroup(std::move(temp));
+}
+
+SequentialCommandGroup Command::AndThen(std::function<void()> toRun) && {
+  std::vector<std::unique_ptr<Command>> temp;
+  temp.emplace_back(std::move(*this).TransferOwnership());
+  temp.emplace_back(std::make_unique<InstantCommand>(
+      std::move(toRun), std::initializer_list<Subsystem*>{}));
+  return SequentialCommandGroup(std::move(temp));
+}
+
+PerpetualCommand Command::Perpetually() && {
+  return PerpetualCommand(std::move(*this).TransferOwnership());
+}
+
+ProxyScheduleCommand Command::AsProxy() { return ProxyScheduleCommand(this); }
+
+void Command::Schedule(bool interruptible) {
+  CommandScheduler::GetInstance().Schedule(interruptible, this);
+}
+
+void Command::Cancel() { CommandScheduler::GetInstance().Cancel(this); }
+
+bool Command::IsScheduled() const {
+  return CommandScheduler::GetInstance().IsScheduled(this);
+}
+
+bool Command::HasRequirement(Subsystem* requirement) const {
+  bool hasRequirement = false;
+  for (auto&& subsystem : GetRequirements()) {
+    hasRequirement |= requirement == subsystem;
+  }
+  return hasRequirement;
+}
+
+std::string Command::GetName() const { return GetTypeName(*this); }
+
+bool Command::IsGrouped() const { return m_isGrouped; }
+
+void Command::SetGrouped(bool grouped) { m_isGrouped = grouped; }
+
+namespace frc2 {
+bool RequirementsDisjoint(Command* first, Command* second) {
+  bool disjoint = true;
+  auto&& requirements = second->GetRequirements();
+  for (auto&& requirement : first->GetRequirements()) {
+    disjoint &= requirements.find(requirement) == requirements.end();
+  }
+  return disjoint;
+}
+}  // namespace frc2

wpilibNewCommands/src/main/native/cpp/frc2/command/CommandBase.cpp

@@ -0,0 +1,62 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/CommandBase.h"
+
+#include <frc/smartdashboard/SendableBuilder.h>
+#include <frc/smartdashboard/SendableRegistry.h>
+#include <frc2/command/CommandScheduler.h>
+#include <frc2/command/SetUtilities.h>
+
+using namespace frc2;
+
+CommandBase::CommandBase() {
+  frc::SendableRegistry::GetInstance().AddLW(this, GetTypeName(*this));
+}
+
+void CommandBase::AddRequirements(
+    std::initializer_list<Subsystem*> requirements) {
+  m_requirements.insert(requirements.begin(), requirements.end());
+}
+
+void CommandBase::AddRequirements(wpi::SmallSet<Subsystem*, 4> requirements) {
+  m_requirements.insert(requirements.begin(), requirements.end());
+}
+
+wpi::SmallSet<Subsystem*, 4> CommandBase::GetRequirements() const {
+  return m_requirements;
+}
+
+void CommandBase::SetName(const wpi::Twine& name) {
+  frc::SendableRegistry::GetInstance().SetName(this, name);
+}
+
+std::string CommandBase::GetName() const {
+  return frc::SendableRegistry::GetInstance().GetName(this);
+}
+
+std::string CommandBase::GetSubsystem() const {
+  return frc::SendableRegistry::GetInstance().GetSubsystem(this);
+}
+
+void CommandBase::SetSubsystem(const wpi::Twine& subsystem) {
+  frc::SendableRegistry::GetInstance().SetSubsystem(this, subsystem);
+}
+
+void CommandBase::InitSendable(frc::SendableBuilder& builder) {
+  builder.SetSmartDashboardType("Command");
+  builder.AddStringProperty(".name", [this] { return GetName(); }, nullptr);
+  builder.AddBooleanProperty("running", [this] { return IsScheduled(); },
+                             [this](bool value) {
+                               bool isScheduled = IsScheduled();
+                               if (value && !isScheduled) {
+                                 Schedule();
+                               } else if (!value && isScheduled) {
+                                 Cancel();
+                               }
+                             });
+}

wpilibNewCommands/src/main/native/cpp/frc2/command/CommandGroupBase.cpp

@@ -0,0 +1,60 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/CommandGroupBase.h"
+
+#include <set>
+
+#include "frc/WPIErrors.h"
+#include "frc2/command/ParallelCommandGroup.h"
+#include "frc2/command/ParallelDeadlineGroup.h"
+#include "frc2/command/ParallelRaceGroup.h"
+#include "frc2/command/SequentialCommandGroup.h"
+
+using namespace frc2;
+template <typename TMap, typename TKey>
+static bool ContainsKey(const TMap& map, TKey keyToCheck) {
+  return map.find(keyToCheck) != map.end();
+}
+bool CommandGroupBase::RequireUngrouped(Command& command) {
+  if (command.IsGrouped()) {
+    wpi_setGlobalWPIErrorWithContext(
+        CommandIllegalUse,
+        "Commands cannot be added to more than one CommandGroup");
+    return false;
+  } else {
+    return true;
+  }
+}
+
+bool CommandGroupBase::RequireUngrouped(
+    wpi::ArrayRef<std::unique_ptr<Command>> commands) {
+  bool allUngrouped = true;
+  for (auto&& command : commands) {
+    allUngrouped &= !command.get()->IsGrouped();
+  }
+  if (!allUngrouped) {
+    wpi_setGlobalWPIErrorWithContext(
+        CommandIllegalUse,
+        "Commands cannot be added to more than one CommandGroup");
+  }
+  return allUngrouped;
+}
+
+bool CommandGroupBase::RequireUngrouped(
+    std::initializer_list<Command*> commands) {
+  bool allUngrouped = true;
+  for (auto&& command : commands) {
+    allUngrouped &= !command->IsGrouped();
+  }
+  if (!allUngrouped) {
+    wpi_setGlobalWPIErrorWithContext(
+        CommandIllegalUse,
+        "Commands cannot be added to more than one CommandGroup");
+  }
+  return allUngrouped;
+}

wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp

@@ -0,0 +1,345 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/CommandScheduler.h"
+
+#include <frc/RobotState.h>
+#include <frc/WPIErrors.h>
+#include <frc/smartdashboard/SendableBuilder.h>
+#include <frc/smartdashboard/SendableRegistry.h>
+#include <frc2/command/CommandGroupBase.h>
+#include <frc2/command/Subsystem.h>
+
+#include <hal/HAL.h>
+
+using namespace frc2;
+template <typename TMap, typename TKey>
+static bool ContainsKey(const TMap& map, TKey keyToCheck) {
+  return map.find(keyToCheck) != map.end();
+}
+
+CommandScheduler::CommandScheduler() {
+  frc::SendableRegistry::GetInstance().AddLW(this, "Scheduler");
+}
+
+CommandScheduler& CommandScheduler::GetInstance() {
+  static CommandScheduler scheduler;
+  return scheduler;
+}
+
+void CommandScheduler::AddButton(wpi::unique_function<void()> button) {
+  m_buttons.emplace_back(std::move(button));
+}
+
+void CommandScheduler::ClearButtons() { m_buttons.clear(); }
+
+void CommandScheduler::Schedule(bool interruptible, Command* command) {
+  if (m_inRunLoop) {
+    m_toSchedule.try_emplace(command, interruptible);
+    return;
+  }
+
+  if (command->IsGrouped()) {
+    wpi_setWPIErrorWithContext(CommandIllegalUse,
+                               "A command that is part of a command group "
+                               "cannot be independently scheduled");
+    return;
+  }
+  if (m_disabled ||
+      (frc::RobotState::IsDisabled() && !command->RunsWhenDisabled()) ||
+      ContainsKey(m_scheduledCommands, command)) {
+    return;
+  }
+
+  const auto& requirements = command->GetRequirements();
+
+  wpi::SmallVector<Command*, 8> intersection;
+
+  bool isDisjoint = true;
+  bool allInterruptible = true;
+  for (auto&& i1 : m_requirements) {
+    if (requirements.find(i1.first) != requirements.end()) {
+      isDisjoint = false;
+      allInterruptible &= m_scheduledCommands[i1.second].IsInterruptible();
+      intersection.emplace_back(i1.second);
+    }
+  }
+
+  if (isDisjoint || allInterruptible) {
+    if (allInterruptible) {
+      for (auto&& cmdToCancel : intersection) {
+        Cancel(cmdToCancel);
+      }
+    }
+    command->Initialize();
+    m_scheduledCommands[command] = CommandState{interruptible};
+    for (auto&& action : m_initActions) {
+      action(*command);
+    }
+    for (auto&& requirement : requirements) {
+      m_requirements[requirement] = command;
+    }
+  }
+}
+
+void CommandScheduler::Schedule(Command* command) { Schedule(true, command); }
+
+void CommandScheduler::Schedule(bool interruptible,
+                                wpi::ArrayRef<Command*> commands) {
+  for (auto command : commands) {
+    Schedule(interruptible, command);
+  }
+}
+
+void CommandScheduler::Schedule(bool interruptible,
+                                std::initializer_list<Command*> commands) {
+  for (auto command : commands) {
+    Schedule(interruptible, command);
+  }
+}
+
+void CommandScheduler::Schedule(wpi::ArrayRef<Command*> commands) {
+  for (auto command : commands) {
+    Schedule(true, command);
+  }
+}
+
+void CommandScheduler::Schedule(std::initializer_list<Command*> commands) {
+  for (auto command : commands) {
+    Schedule(true, command);
+  }
+}
+
+void CommandScheduler::Run() {
+  if (m_disabled) {
+    return;
+  }
+
+  // Run the periodic method of all registered subsystems.
+  for (auto&& subsystem : m_subsystems) {
+    subsystem.getFirst()->Periodic();
+  }
+
+  // Poll buttons for new commands to add.
+  for (auto&& button : m_buttons) {
+    button();
+  }
+
+  m_inRunLoop = true;
+  // Run scheduled commands, remove finished commands.
+  for (auto iterator = m_scheduledCommands.begin();
+       iterator != m_scheduledCommands.end(); iterator++) {
+    Command* command = iterator->getFirst();
+
+    if (!command->RunsWhenDisabled() && frc::RobotState::IsDisabled()) {
+      Cancel(command);
+      continue;
+    }
+
+    command->Execute();
+    for (auto&& action : m_executeActions) {
+      action(*command);
+    }
+
+    if (command->IsFinished()) {
+      command->End(false);
+      for (auto&& action : m_finishActions) {
+        action(*command);
+      }
+
+      for (auto&& requirement : command->GetRequirements()) {
+        m_requirements.erase(requirement);
+      }
+
+      m_scheduledCommands.erase(iterator);
+    }
+  }
+  m_inRunLoop = false;
+
+  for (auto&& commandInterruptible : m_toSchedule) {
+    Schedule(commandInterruptible.second, commandInterruptible.first);
+  }
+
+  for (auto&& command : m_toCancel) {
+    Cancel(command);
+  }
+
+  m_toSchedule.clear();
+  m_toCancel.clear();
+
+  // Add default commands for un-required registered subsystems.
+  for (auto&& subsystem : m_subsystems) {
+    auto s = m_requirements.find(subsystem.getFirst());
+    if (s == m_requirements.end()) {
+      Schedule({subsystem.getSecond().get()});
+    }
+  }
+}
+
+void CommandScheduler::RegisterSubsystem(Subsystem* subsystem) {
+  m_subsystems[subsystem] = nullptr;
+}
+
+void CommandScheduler::UnregisterSubsystem(Subsystem* subsystem) {
+  auto s = m_subsystems.find(subsystem);
+  if (s != m_subsystems.end()) {
+    m_subsystems.erase(s);
+  }
+}
+
+void CommandScheduler::RegisterSubsystem(
+    std::initializer_list<Subsystem*> subsystems) {
+  for (auto* subsystem : subsystems) {
+    RegisterSubsystem(subsystem);
+  }
+}
+
+void CommandScheduler::UnregisterSubsystem(
+    std::initializer_list<Subsystem*> subsystems) {
+  for (auto* subsystem : subsystems) {
+    UnregisterSubsystem(subsystem);
+  }
+}
+
+Command* CommandScheduler::GetDefaultCommand(const Subsystem* subsystem) const {
+  auto&& find = m_subsystems.find(subsystem);
+  if (find != m_subsystems.end()) {
+    return find->second.get();
+  } else {
+    return nullptr;
+  }
+}
+
+void CommandScheduler::Cancel(Command* command) {
+  if (m_inRunLoop) {
+    m_toCancel.emplace_back(command);
+    return;
+  }
+
+  auto find = m_scheduledCommands.find(command);
+  if (find == m_scheduledCommands.end()) return;
+  command->End(true);
+  for (auto&& action : m_interruptActions) {
+    action(*command);
+  }
+  m_scheduledCommands.erase(find);
+  for (auto&& requirement : m_requirements) {
+    if (requirement.second == command) {
+      m_requirements.erase(requirement.first);
+    }
+  }
+}
+
+void CommandScheduler::Cancel(wpi::ArrayRef<Command*> commands) {
+  for (auto command : commands) {
+    Cancel(command);
+  }
+}
+
+void CommandScheduler::Cancel(std::initializer_list<Command*> commands) {
+  for (auto command : commands) {
+    Cancel(command);
+  }
+}
+
+void CommandScheduler::CancelAll() {
+  for (auto&& command : m_scheduledCommands) {
+    Cancel(command.first);
+  }
+}
+
+double CommandScheduler::TimeSinceScheduled(const Command* command) const {
+  auto find = m_scheduledCommands.find(command);
+  if (find != m_scheduledCommands.end()) {
+    return find->second.TimeSinceInitialized();
+  } else {
+    return -1;
+  }
+}
+bool CommandScheduler::IsScheduled(
+    wpi::ArrayRef<const Command*> commands) const {
+  for (auto command : commands) {
+    if (!IsScheduled(command)) {
+      return false;
+    }
+  }
+  return true;
+}
+
+bool CommandScheduler::IsScheduled(
+    std::initializer_list<const Command*> commands) const {
+  for (auto command : commands) {
+    if (!IsScheduled(command)) {
+      return false;
+    }
+  }
+  return true;
+}
+
+bool CommandScheduler::IsScheduled(const Command* command) const {
+  return m_scheduledCommands.find(command) != m_scheduledCommands.end();
+}
+
+Command* CommandScheduler::Requiring(const Subsystem* subsystem) const {
+  auto find = m_requirements.find(subsystem);
+  if (find != m_requirements.end()) {
+    return find->second;
+  } else {
+    return nullptr;
+  }
+}
+
+void CommandScheduler::Disable() { m_disabled = true; }
+
+void CommandScheduler::Enable() { m_disabled = false; }
+
+void CommandScheduler::OnCommandInitialize(Action action) {
+  m_initActions.emplace_back(std::move(action));
+}
+
+void CommandScheduler::OnCommandExecute(Action action) {
+  m_executeActions.emplace_back(std::move(action));
+}
+
+void CommandScheduler::OnCommandInterrupt(Action action) {
+  m_interruptActions.emplace_back(std::move(action));
+}
+
+void CommandScheduler::OnCommandFinish(Action action) {
+  m_finishActions.emplace_back(std::move(action));
+}
+
+void CommandScheduler::InitSendable(frc::SendableBuilder& builder) {
+  builder.SetSmartDashboardType("Scheduler");
+  m_namesEntry = builder.GetEntry("Names");
+  m_idsEntry = builder.GetEntry("Ids");
+  m_cancelEntry = builder.GetEntry("Cancel");
+
+  builder.SetUpdateTable([this] {
+    double tmp[1];
+    tmp[0] = 0;
+    auto toCancel = m_cancelEntry.GetDoubleArray(tmp);
+    for (auto cancel : toCancel) {
+      uintptr_t ptrTmp = static_cast<uintptr_t>(cancel);
+      Command* command = reinterpret_cast<Command*>(ptrTmp);
+      if (m_scheduledCommands.find(command) != m_scheduledCommands.end()) {
+        Cancel(command);
+      }
+      m_cancelEntry.SetDoubleArray(wpi::ArrayRef<double>{});
+    }
+
+    wpi::SmallVector<std::string, 8> names;
+    wpi::SmallVector<double, 8> ids;
+    for (auto&& command : m_scheduledCommands) {
+      names.emplace_back(command.first->GetName());
+      uintptr_t ptrTmp = reinterpret_cast<uintptr_t>(command.first);
+      ids.emplace_back(static_cast<double>(ptrTmp));
+    }
+    m_namesEntry.SetStringArray(names);
+    m_idsEntry.SetDoubleArray(ids);
+  });
+}

wpilibNewCommands/src/main/native/cpp/frc2/command/CommandState.cpp

@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/CommandState.h"
+
+#include "frc/Timer.h"
+
+using namespace frc2;
+CommandState::CommandState(bool interruptible)
+    : m_interruptible{interruptible} {
+  StartTiming();
+  StartRunning();
+}
+
+void CommandState::StartTiming() {
+  m_startTime = frc::Timer::GetFPGATimestamp();
+}
+void CommandState::StartRunning() { m_startTime = -1; }
+double CommandState::TimeSinceInitialized() const {
+  return m_startTime != -1 ? frc::Timer::GetFPGATimestamp() - m_startTime : -1;
+}

wpilibNewCommands/src/main/native/cpp/frc2/command/ConditionalCommand.cpp

@@ -0,0 +1,52 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/ConditionalCommand.h"
+
+using namespace frc2;
+
+ConditionalCommand::ConditionalCommand(std::unique_ptr<Command>&& onTrue,
+                                       std::unique_ptr<Command>&& onFalse,
+                                       std::function<bool()> condition)
+    : m_condition{std::move(condition)} {
+  if (!CommandGroupBase::RequireUngrouped({onTrue.get(), onFalse.get()})) {
+    return;
+  }
+
+  m_onTrue = std::move(onTrue);
+  m_onFalse = std::move(onFalse);
+
+  m_onTrue->SetGrouped(true);
+  m_onFalse->SetGrouped(true);
+
+  m_runsWhenDisabled &= m_onTrue->RunsWhenDisabled();
+  m_runsWhenDisabled &= m_onFalse->RunsWhenDisabled();
+
+  AddRequirements(m_onTrue->GetRequirements());
+  AddRequirements(m_onFalse->GetRequirements());
+}
+
+void ConditionalCommand::Initialize() {
+  if (m_condition()) {
+    m_selectedCommand = m_onTrue.get();
+  } else {
+    m_selectedCommand = m_onFalse.get();
+  }
+  m_selectedCommand->Initialize();
+}
+
+void ConditionalCommand::Execute() { m_selectedCommand->Execute(); }
+
+void ConditionalCommand::End(bool interrupted) {
+  m_selectedCommand->End(interrupted);
+}
+
+bool ConditionalCommand::IsFinished() {
+  return m_selectedCommand->IsFinished();
+}
+
+bool ConditionalCommand::RunsWhenDisabled() const { return m_runsWhenDisabled; }

wpilibNewCommands/src/main/native/cpp/frc2/command/FunctionalCommand.cpp

@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/FunctionalCommand.h"
+
+using namespace frc2;
+
+FunctionalCommand::FunctionalCommand(std::function<void()> onInit,
+                                     std::function<void()> onExecute,
+                                     std::function<void(bool)> onEnd,
+                                     std::function<bool()> isFinished)
+    : m_onInit{std::move(onInit)},
+      m_onExecute{std::move(onExecute)},
+      m_onEnd{std::move(onEnd)},
+      m_isFinished{std::move(isFinished)} {}
+
+void FunctionalCommand::Initialize() { m_onInit(); }
+
+void FunctionalCommand::Execute() { m_onExecute(); }
+
+void FunctionalCommand::End(bool interrupted) { m_onEnd(interrupted); }
+
+bool FunctionalCommand::IsFinished() { return m_isFinished(); }

wpilibNewCommands/src/main/native/cpp/frc2/command/InstantCommand.cpp

@@ -0,0 +1,22 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/InstantCommand.h"
+
+using namespace frc2;
+
+InstantCommand::InstantCommand(std::function<void()> toRun,
+                               std::initializer_list<Subsystem*> requirements)
+    : m_toRun{std::move(toRun)} {
+  AddRequirements(requirements);
+}
+
+InstantCommand::InstantCommand() : m_toRun{[] {}} {}
+
+void InstantCommand::Initialize() { m_toRun(); }
+
+bool InstantCommand::IsFinished() { return true; }

wpilibNewCommands/src/main/native/cpp/frc2/command/NotifierCommand.cpp

@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/NotifierCommand.h"
+
+using namespace frc2;
+
+NotifierCommand::NotifierCommand(std::function<void()> toRun,
+                                 units::second_t period,
+                                 std::initializer_list<Subsystem*> requirements)
+    : m_toRun(toRun), m_notifier{std::move(toRun)}, m_period{period} {
+  AddRequirements(requirements);
+}
+
+NotifierCommand::NotifierCommand(NotifierCommand&& other)
+    : CommandHelper(std::move(other)),
+      m_toRun(other.m_toRun),
+      m_notifier(other.m_toRun),
+      m_period(other.m_period) {}
+
+NotifierCommand::NotifierCommand(const NotifierCommand& other)
+    : CommandHelper(other),
+      m_toRun(other.m_toRun),
+      m_notifier(frc::Notifier(other.m_toRun)),
+      m_period(other.m_period) {}
+
+void NotifierCommand::Initialize() { m_notifier.StartPeriodic(m_period); }
+
+void NotifierCommand::End(bool interrupted) { m_notifier.Stop(); }

wpilibNewCommands/src/main/native/cpp/frc2/command/PIDCommand.cpp

@@ -0,0 +1,39 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/PIDCommand.h"
+
+using namespace frc2;
+
+PIDCommand::PIDCommand(PIDController controller,
+                       std::function<double()> measurementSource,
+                       std::function<double()> setpointSource,
+                       std::function<void(double)> useOutput,
+                       std::initializer_list<Subsystem*> requirements)
+    : m_controller{controller},
+      m_measurement{std::move(measurementSource)},
+      m_setpoint{std::move(setpointSource)},
+      m_useOutput{std::move(useOutput)} {
+  AddRequirements(requirements);
+}
+
+PIDCommand::PIDCommand(PIDController controller,
+                       std::function<double()> measurementSource,
+                       double setpoint, std::function<void(double)> useOutput,
+                       std::initializer_list<Subsystem*> requirements)
+    : PIDCommand(controller, measurementSource, [setpoint] { return setpoint; },
+                 useOutput, requirements) {}
+
+void PIDCommand::Initialize() { m_controller.Reset(); }
+
+void PIDCommand::Execute() {
+  m_useOutput(m_controller.Calculate(m_measurement(), m_setpoint()));
+}
+
+void PIDCommand::End(bool interrupted) { m_useOutput(0); }
+
+PIDController& PIDCommand::getController() { return m_controller; }

wpilibNewCommands/src/main/native/cpp/frc2/command/PIDSubsystem.cpp

@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/PIDSubsystem.h"
+
+using namespace frc2;
+
+PIDSubsystem::PIDSubsystem(PIDController controller)
+    : m_controller{controller} {}
+
+void PIDSubsystem::Periodic() {
+  if (m_enabled) {
+    UseOutput(m_controller.Calculate(GetMeasurement(), GetSetpoint()));
+  }
+}
+
+void PIDSubsystem::Enable() {
+  m_controller.Reset();
+  m_enabled = true;
+}
+
+void PIDSubsystem::Disable() {
+  UseOutput(0);
+  m_enabled = false;
+}
+
+PIDController& PIDSubsystem::GetController() { return m_controller; }

wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelCommandGroup.cpp

@@ -0,0 +1,83 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/ParallelCommandGroup.h"
+
+using namespace frc2;
+
+ParallelCommandGroup::ParallelCommandGroup(
+    std::vector<std::unique_ptr<Command>>&& commands) {
+  AddCommands(std::move(commands));
+}
+
+void ParallelCommandGroup::Initialize() {
+  for (auto& commandRunning : m_commands) {
+    commandRunning.first->Initialize();
+    commandRunning.second = true;
+  }
+  isRunning = true;
+}
+
+void ParallelCommandGroup::Execute() {
+  for (auto& commandRunning : m_commands) {
+    if (!commandRunning.second) continue;
+    commandRunning.first->Execute();
+    if (commandRunning.first->IsFinished()) {
+      commandRunning.first->End(false);
+      commandRunning.second = false;
+    }
+  }
+}
+
+void ParallelCommandGroup::End(bool interrupted) {
+  if (interrupted) {
+    for (auto& commandRunning : m_commands) {
+      if (commandRunning.second) {
+        commandRunning.first->End(true);
+      }
+    }
+  }
+  isRunning = false;
+}
+
+bool ParallelCommandGroup::IsFinished() {
+  for (auto& command : m_commands) {
+    if (command.second) return false;
+  }
+  return true;
+}
+
+bool ParallelCommandGroup::RunsWhenDisabled() const {
+  return m_runWhenDisabled;
+}
+
+void ParallelCommandGroup::AddCommands(
+    std::vector<std::unique_ptr<Command>>&& commands) {
+  for (auto&& command : commands) {
+    if (!RequireUngrouped(*command)) return;
+  }
+
+  if (isRunning) {
+    wpi_setWPIErrorWithContext(CommandIllegalUse,
+                               "Commands cannot be added to a CommandGroup "
+                               "while the group is running");
+  }
+
+  for (auto&& command : commands) {
+    if (RequirementsDisjoint(this, command.get())) {
+      command->SetGrouped(true);
+      AddRequirements(command->GetRequirements());
+      m_runWhenDisabled &= command->RunsWhenDisabled();
+      m_commands[std::move(command)] = false;
+    } else {
+      wpi_setWPIErrorWithContext(CommandIllegalUse,
+                                 "Multiple commands in a parallel group cannot "
+                                 "require the same subsystems");
+      return;
+    }
+  }
+}

wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelDeadlineGroup.cpp

@@ -0,0 +1,86 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/ParallelDeadlineGroup.h"
+
+using namespace frc2;
+
+ParallelDeadlineGroup::ParallelDeadlineGroup(
+    std::unique_ptr<Command>&& deadline,
+    std::vector<std::unique_ptr<Command>>&& commands) {
+  SetDeadline(std::move(deadline));
+  AddCommands(std::move(commands));
+}
+
+void ParallelDeadlineGroup::Initialize() {
+  for (auto& commandRunning : m_commands) {
+    commandRunning.first->Initialize();
+    commandRunning.second = true;
+  }
+  isRunning = true;
+}
+
+void ParallelDeadlineGroup::Execute() {
+  for (auto& commandRunning : m_commands) {
+    if (!commandRunning.second) continue;
+    commandRunning.first->Execute();
+    if (commandRunning.first->IsFinished()) {
+      commandRunning.first->End(false);
+      commandRunning.second = false;
+    }
+  }
+}
+
+void ParallelDeadlineGroup::End(bool interrupted) {
+  for (auto& commandRunning : m_commands) {
+    if (commandRunning.second) {
+      commandRunning.first->End(true);
+    }
+  }
+  isRunning = false;
+}
+
+bool ParallelDeadlineGroup::IsFinished() { return m_deadline->IsFinished(); }
+
+bool ParallelDeadlineGroup::RunsWhenDisabled() const {
+  return m_runWhenDisabled;
+}
+
+void ParallelDeadlineGroup::AddCommands(
+    std::vector<std::unique_ptr<Command>>&& commands) {
+  if (!RequireUngrouped(commands)) {
+    return;
+  }
+
+  if (isRunning) {
+    wpi_setWPIErrorWithContext(CommandIllegalUse,
+                               "Commands cannot be added to a CommandGroup "
+                               "while the group is running");
+  }
+
+  for (auto&& command : commands) {
+    if (RequirementsDisjoint(this, command.get())) {
+      command->SetGrouped(true);
+      AddRequirements(command->GetRequirements());
+      m_runWhenDisabled &= command->RunsWhenDisabled();
+      m_commands[std::move(command)] = false;
+    } else {
+      wpi_setWPIErrorWithContext(CommandIllegalUse,
+                                 "Multiple commands in a parallel group cannot "
+                                 "require the same subsystems");
+      return;
+    }
+  }
+}
+
+void ParallelDeadlineGroup::SetDeadline(std::unique_ptr<Command>&& deadline) {
+  m_deadline = deadline.get();
+  m_deadline->SetGrouped(true);
+  m_commands[std::move(deadline)] = false;
+  AddRequirements(m_deadline->GetRequirements());
+  m_runWhenDisabled &= m_deadline->RunsWhenDisabled();
+}

wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp

@@ -0,0 +1,69 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/ParallelRaceGroup.h"
+
+using namespace frc2;
+
+ParallelRaceGroup::ParallelRaceGroup(
+    std::vector<std::unique_ptr<Command>>&& commands) {
+  AddCommands(std::move(commands));
+}
+
+void ParallelRaceGroup::Initialize() {
+  for (auto& commandRunning : m_commands) {
+    commandRunning->Initialize();
+  }
+  isRunning = true;
+}
+
+void ParallelRaceGroup::Execute() {
+  for (auto& commandRunning : m_commands) {
+    commandRunning->Execute();
+    if (commandRunning->IsFinished()) {
+      m_finished = true;
+    }
+  }
+}
+
+void ParallelRaceGroup::End(bool interrupted) {
+  for (auto& commandRunning : m_commands) {
+    commandRunning->End(!commandRunning->IsFinished());
+  }
+  isRunning = false;
+}
+
+bool ParallelRaceGroup::IsFinished() { return m_finished; }
+
+bool ParallelRaceGroup::RunsWhenDisabled() const { return m_runWhenDisabled; }
+
+void ParallelRaceGroup::AddCommands(
+    std::vector<std::unique_ptr<Command>>&& commands) {
+  if (!RequireUngrouped(commands)) {
+    return;
+  }
+
+  if (isRunning) {
+    wpi_setWPIErrorWithContext(CommandIllegalUse,
+                               "Commands cannot be added to a CommandGroup "
+                               "while the group is running");
+  }
+
+  for (auto&& command : commands) {
+    if (RequirementsDisjoint(this, command.get())) {
+      command->SetGrouped(true);
+      AddRequirements(command->GetRequirements());
+      m_runWhenDisabled &= command->RunsWhenDisabled();
+      m_commands.emplace(std::move(command));
+    } else {
+      wpi_setWPIErrorWithContext(CommandIllegalUse,
+                                 "Multiple commands in a parallel group cannot "
+                                 "require the same subsystems");
+      return;
+    }
+  }
+}

wpilibNewCommands/src/main/native/cpp/frc2/command/PerpetualCommand.cpp

@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/PerpetualCommand.h"
+
+using namespace frc2;
+
+PerpetualCommand::PerpetualCommand(std::unique_ptr<Command>&& command) {
+  if (!CommandGroupBase::RequireUngrouped(command)) {
+    return;
+  }
+  m_command = std::move(command);
+  m_command->SetGrouped(true);
+  AddRequirements(m_command->GetRequirements());
+}
+
+void PerpetualCommand::Initialize() { m_command->Initialize(); }
+
+void PerpetualCommand::Execute() { m_command->Execute(); }
+
+void PerpetualCommand::End(bool interrupted) { m_command->End(interrupted); }

wpilibNewCommands/src/main/native/cpp/frc2/command/PrintCommand.cpp

@@ -0,0 +1,16 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/PrintCommand.h"
+
+using namespace frc2;
+
+PrintCommand::PrintCommand(const wpi::Twine& message)
+    : CommandHelper{[str = message.str()] { wpi::outs() << str << "\n"; }, {}} {
+}
+
+bool PrintCommand::RunsWhenDisabled() const { return true; }

wpilibNewCommands/src/main/native/cpp/frc2/command/ProfiledPIDCommand.cpp

@@ -0,0 +1,67 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/ProfiledPIDCommand.h"
+
+using namespace frc2;
+using State = frc::TrapezoidProfile::State;
+
+ProfiledPIDCommand::ProfiledPIDCommand(
+    frc::ProfiledPIDController controller,
+    std::function<units::meter_t()> measurementSource,
+    std::function<State()> goalSource,
+    std::function<void(double, State)> useOutput,
+    std::initializer_list<Subsystem*> requirements)
+    : m_controller{controller},
+      m_measurement{std::move(measurementSource)},
+      m_goal{std::move(goalSource)},
+      m_useOutput{std::move(useOutput)} {
+  AddRequirements(requirements);
+}
+
+ProfiledPIDCommand::ProfiledPIDCommand(
+    frc::ProfiledPIDController controller,
+    std::function<units::meter_t()> measurementSource,
+    std::function<units::meter_t()> goalSource,
+    std::function<void(double, State)> useOutput,
+    std::initializer_list<Subsystem*> requirements)
+    : ProfiledPIDCommand(controller, measurementSource,
+                         [&goalSource]() {
+                           return State{goalSource(), 0_mps};
+                         },
+                         useOutput, requirements) {}
+
+ProfiledPIDCommand::ProfiledPIDCommand(
+    frc::ProfiledPIDController controller,
+    std::function<units::meter_t()> measurementSource, State goal,
+    std::function<void(double, State)> useOutput,
+    std::initializer_list<Subsystem*> requirements)
+    : ProfiledPIDCommand(controller, measurementSource, [goal] { return goal; },
+                         useOutput, requirements) {}
+
+ProfiledPIDCommand::ProfiledPIDCommand(
+    frc::ProfiledPIDController controller,
+    std::function<units::meter_t()> measurementSource, units::meter_t goal,
+    std::function<void(double, State)> useOutput,
+    std::initializer_list<Subsystem*> requirements)
+    : ProfiledPIDCommand(controller, measurementSource, [goal] { return goal; },
+                         useOutput, requirements) {}
+
+void ProfiledPIDCommand::Initialize() { m_controller.Reset(); }
+
+void ProfiledPIDCommand::Execute() {
+  m_useOutput(m_controller.Calculate(m_measurement(), m_goal()),
+              m_controller.GetSetpoint());
+}
+
+void ProfiledPIDCommand::End(bool interrupted) {
+  m_useOutput(0, State{0_m, 0_mps});
+}
+
+frc::ProfiledPIDController& ProfiledPIDCommand::GetController() {
+  return m_controller;
+}

wpilibNewCommands/src/main/native/cpp/frc2/command/ProfiledPIDSubsystem.cpp

@@ -0,0 +1,36 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/ProfiledPIDSubsystem.h"
+
+using namespace frc2;
+using State = frc::TrapezoidProfile::State;
+
+ProfiledPIDSubsystem::ProfiledPIDSubsystem(
+    frc::ProfiledPIDController controller)
+    : m_controller{controller} {}
+
+void ProfiledPIDSubsystem::Periodic() {
+  if (m_enabled) {
+    UseOutput(m_controller.Calculate(GetMeasurement(), GetGoal()),
+              m_controller.GetSetpoint());
+  }
+}
+
+void ProfiledPIDSubsystem::Enable() {
+  m_controller.Reset();
+  m_enabled = true;
+}
+
+void ProfiledPIDSubsystem::Disable() {
+  UseOutput(0, State{0_m, 0_mps});
+  m_enabled = false;
+}
+
+frc::ProfiledPIDController& ProfiledPIDSubsystem::GetController() {
+  return m_controller;
+}

wpilibNewCommands/src/main/native/cpp/frc2/command/ProxyScheduleCommand.cpp

@@ -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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/ProxyScheduleCommand.h"
+
+using namespace frc2;
+
+ProxyScheduleCommand::ProxyScheduleCommand(wpi::ArrayRef<Command*> toSchedule) {
+  SetInsert(m_toSchedule, toSchedule);
+}
+
+void ProxyScheduleCommand::Initialize() {
+  for (auto* command : m_toSchedule) {
+    command->Schedule();
+  }
+}
+
+void ProxyScheduleCommand::End(bool interrupted) {
+  if (interrupted) {
+    for (auto* command : m_toSchedule) {
+      command->Cancel();
+    }
+  }
+}
+
+void ProxyScheduleCommand::Execute() {
+  m_finished = true;
+  for (auto* command : m_toSchedule) {
+    m_finished &= !command->IsScheduled();
+  }
+}
+
+bool ProxyScheduleCommand::IsFinished() { return m_finished; }

wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp

@@ -0,0 +1,117 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/RamseteCommand.h"
+
+using namespace frc2;
+using namespace units;
+
+template <typename T>
+int sgn(T val) {
+  return (T(0) < val) - (val < T(0));
+}
+
+RamseteCommand::RamseteCommand(
+    frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+    frc::RamseteController controller, volt_t ks,
+    units::unit_t<voltsecondspermeter> kv,
+    units::unit_t<voltsecondssquaredpermeter> ka,
+    frc::DifferentialDriveKinematics kinematics,
+    std::function<units::meters_per_second_t()> leftSpeed,
+    std::function<units::meters_per_second_t()> rightSpeed,
+    frc2::PIDController leftController, frc2::PIDController rightController,
+    std::function<void(volt_t, volt_t)> output,
+    std::initializer_list<Subsystem*> requirements)
+    : m_trajectory(trajectory),
+      m_pose(pose),
+      m_controller(controller),
+      m_ks(ks),
+      m_kv(kv),
+      m_ka(ka),
+      m_kinematics(kinematics),
+      m_leftSpeed(leftSpeed),
+      m_rightSpeed(rightSpeed),
+      m_leftController(std::make_unique<frc2::PIDController>(leftController)),
+      m_rightController(std::make_unique<frc2::PIDController>(rightController)),
+      m_outputVolts(output),
+      m_usePID(true) {
+  AddRequirements(requirements);
+}
+
+RamseteCommand::RamseteCommand(
+    frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+    frc::RamseteController controller,
+    frc::DifferentialDriveKinematics kinematics,
+    std::function<void(units::meters_per_second_t, units::meters_per_second_t)>
+        output,
+    std::initializer_list<Subsystem*> requirements)
+    : m_trajectory(trajectory),
+      m_pose(pose),
+      m_controller(controller),
+      m_ks(0),
+      m_kv(0),
+      m_ka(0),
+      m_kinematics(kinematics),
+      m_outputVel(output),
+      m_usePID(false) {
+  AddRequirements(requirements);
+}
+
+void RamseteCommand::Initialize() {
+  m_prevTime = 0_s;
+  auto initialState = m_trajectory.Sample(0_s);
+  m_prevSpeeds = m_kinematics.ToWheelSpeeds(
+      frc::ChassisSpeeds{initialState.velocity, 0_mps,
+                         initialState.velocity * initialState.curvature});
+  m_timer.Reset();
+  m_timer.Start();
+  if (m_usePID) {
+    m_leftController->Reset();
+    m_rightController->Reset();
+  }
+}
+
+void RamseteCommand::Execute() {
+  auto curTime = m_timer.Get();
+  auto dt = curTime - m_prevTime;
+
+  auto targetWheelSpeeds = m_kinematics.ToWheelSpeeds(
+      m_controller.Calculate(m_pose(), m_trajectory.Sample(curTime)));
+
+  if (m_usePID) {
+    auto leftFeedforward =
+        m_ks * sgn(targetWheelSpeeds.left) + m_kv * targetWheelSpeeds.left +
+        m_ka * (targetWheelSpeeds.left - m_prevSpeeds.left) / dt;
+
+    auto rightFeedforward =
+        m_ks * sgn(targetWheelSpeeds.right) + m_kv * targetWheelSpeeds.right +
+        m_ka * (targetWheelSpeeds.right - m_prevSpeeds.right) / dt;
+
+    auto leftOutput =
+        volt_t(m_leftController->Calculate(
+            m_leftSpeed().to<double>(), targetWheelSpeeds.left.to<double>())) +
+        leftFeedforward;
+
+    auto rightOutput = volt_t(m_rightController->Calculate(
+                           m_rightSpeed().to<double>(),
+                           targetWheelSpeeds.right.to<double>())) +
+                       rightFeedforward;
+
+    m_outputVolts(leftOutput, rightOutput);
+  } else {
+    m_outputVel(targetWheelSpeeds.left, targetWheelSpeeds.right);
+  }
+
+  m_prevTime = curTime;
+  m_prevSpeeds = targetWheelSpeeds;
+}
+
+void RamseteCommand::End(bool interrupted) { m_timer.Stop(); }
+
+bool RamseteCommand::IsFinished() {
+  return m_timer.HasPeriodPassed(m_trajectory.TotalTime());
+}

wpilibNewCommands/src/main/native/cpp/frc2/command/RunCommand.cpp

@@ -0,0 +1,18 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/RunCommand.h"
+
+using namespace frc2;
+
+RunCommand::RunCommand(std::function<void()> toRun,
+                       std::initializer_list<Subsystem*> requirements)
+    : m_toRun{std::move(toRun)} {
+  AddRequirements(requirements);
+}
+
+void RunCommand::Execute() { m_toRun(); }

wpilibNewCommands/src/main/native/cpp/frc2/command/ScheduleCommand.cpp

@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/ScheduleCommand.h"
+
+using namespace frc2;
+
+ScheduleCommand::ScheduleCommand(wpi::ArrayRef<Command*> toSchedule) {
+  SetInsert(m_toSchedule, toSchedule);
+}
+
+void ScheduleCommand::Initialize() {
+  for (auto command : m_toSchedule) {
+    command->Schedule();
+  }
+}
+
+bool ScheduleCommand::IsFinished() { return true; }
+
+bool ScheduleCommand::RunsWhenDisabled() const { return true; }

wpilibNewCommands/src/main/native/cpp/frc2/command/SequentialCommandGroup.cpp

@@ -0,0 +1,75 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/SequentialCommandGroup.h"
+
+using namespace frc2;
+
+SequentialCommandGroup::SequentialCommandGroup(
+    std::vector<std::unique_ptr<Command>>&& commands) {
+  AddCommands(std::move(commands));
+}
+
+void SequentialCommandGroup::Initialize() {
+  m_currentCommandIndex = 0;
+
+  if (!m_commands.empty()) {
+    m_commands[0]->Initialize();
+  }
+}
+
+void SequentialCommandGroup::Execute() {
+  if (m_commands.empty()) return;
+
+  auto& currentCommand = m_commands[m_currentCommandIndex];
+
+  currentCommand->Execute();
+  if (currentCommand->IsFinished()) {
+    currentCommand->End(false);
+    m_currentCommandIndex++;
+    if (m_currentCommandIndex < m_commands.size()) {
+      m_commands[m_currentCommandIndex]->Initialize();
+    }
+  }
+}
+
+void SequentialCommandGroup::End(bool interrupted) {
+  if (interrupted && !m_commands.empty() &&
+      m_currentCommandIndex != invalid_index &&
+      m_currentCommandIndex < m_commands.size()) {
+    m_commands[m_currentCommandIndex]->End(interrupted);
+  }
+  m_currentCommandIndex = invalid_index;
+}
+
+bool SequentialCommandGroup::IsFinished() {
+  return m_currentCommandIndex == m_commands.size();
+}
+
+bool SequentialCommandGroup::RunsWhenDisabled() const {
+  return m_runWhenDisabled;
+}
+
+void SequentialCommandGroup::AddCommands(
+    std::vector<std::unique_ptr<Command>>&& commands) {
+  if (!RequireUngrouped(commands)) {
+    return;
+  }
+
+  if (m_currentCommandIndex != invalid_index) {
+    wpi_setWPIErrorWithContext(CommandIllegalUse,
+                               "Commands cannot be added to a CommandGroup "
+                               "while the group is running");
+  }
+
+  for (auto&& command : commands) {
+    command->SetGrouped(true);
+    AddRequirements(command->GetRequirements());
+    m_runWhenDisabled &= command->RunsWhenDisabled();
+    m_commands.emplace_back(std::move(command));
+  }
+}

wpilibNewCommands/src/main/native/cpp/frc2/command/StartEndCommand.cpp

@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/StartEndCommand.h"
+
+using namespace frc2;
+
+StartEndCommand::StartEndCommand(std::function<void()> onInit,
+                                 std::function<void()> onEnd,
+                                 std::initializer_list<Subsystem*> requirements)
+    : m_onInit{std::move(onInit)}, m_onEnd{std::move(onEnd)} {
+  AddRequirements(requirements);
+}
+
+StartEndCommand::StartEndCommand(const StartEndCommand& other)
+    : CommandHelper(other) {
+  m_onInit = other.m_onInit;
+  m_onEnd = other.m_onEnd;
+}
+
+void StartEndCommand::Initialize() { m_onInit(); }
+
+void StartEndCommand::End(bool interrupted) { m_onEnd(); }

wpilibNewCommands/src/main/native/cpp/frc2/command/Subsystem.cpp

@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/Subsystem.h"
+
+using namespace frc2;
+Subsystem::~Subsystem() {
+  CommandScheduler::GetInstance().UnregisterSubsystem(this);
+}
+
+void Subsystem::Periodic() {}
+
+Command* Subsystem::GetDefaultCommand() const {
+  return CommandScheduler::GetInstance().GetDefaultCommand(this);
+}
+
+Command* Subsystem::GetCurrentCommand() const {
+  return CommandScheduler::GetInstance().Requiring(this);
+}
+
+void Subsystem::Register() {
+  return CommandScheduler::GetInstance().RegisterSubsystem(this);
+}

wpilibNewCommands/src/main/native/cpp/frc2/command/SubsystemBase.cpp

@@ -0,0 +1,66 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/SubsystemBase.h"
+
+#include <frc/smartdashboard/SendableBuilder.h>
+#include <frc/smartdashboard/SendableRegistry.h>
+#include <frc2/command/Command.h>
+#include <frc2/command/CommandScheduler.h>
+
+using namespace frc2;
+
+SubsystemBase::SubsystemBase() {
+  frc::SendableRegistry::GetInstance().AddLW(this, GetTypeName(*this));
+  CommandScheduler::GetInstance().RegisterSubsystem({this});
+}
+
+void SubsystemBase::InitSendable(frc::SendableBuilder& builder) {
+  builder.SetSmartDashboardType("Subsystem");
+  builder.AddBooleanProperty(".hasDefault",
+                             [this] { return GetDefaultCommand() != nullptr; },
+                             nullptr);
+  builder.AddStringProperty(".default",
+                            [this]() -> std::string {
+                              auto command = GetDefaultCommand();
+                              if (command == nullptr) return "none";
+                              return command->GetName();
+                            },
+                            nullptr);
+  builder.AddBooleanProperty(".hasCommand",
+                             [this] { return GetCurrentCommand() != nullptr; },
+                             nullptr);
+  builder.AddStringProperty(".command",
+                            [this]() -> std::string {
+                              auto command = GetCurrentCommand();
+                              if (command == nullptr) return "none";
+                              return command->GetName();
+                            },
+                            nullptr);
+}
+
+std::string SubsystemBase::GetName() const {
+  return frc::SendableRegistry::GetInstance().GetName(this);
+}
+
+void SubsystemBase::SetName(const wpi::Twine& name) {
+  frc::SendableRegistry::GetInstance().SetName(this, name);
+}
+
+std::string SubsystemBase::GetSubsystem() const {
+  return frc::SendableRegistry::GetInstance().GetSubsystem(this);
+}
+
+void SubsystemBase::SetSubsystem(const wpi::Twine& name) {
+  frc::SendableRegistry::GetInstance().SetSubsystem(this, name);
+}
+
+void SubsystemBase::AddChild(std::string name, frc::Sendable* child) {
+  auto& registry = frc::SendableRegistry::GetInstance();
+  registry.AddLW(child, GetSubsystem(), name);
+  registry.AddChild(this, child);
+}

wpilibNewCommands/src/main/native/cpp/frc2/command/TrapezoidProfileCommand.cpp

@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/TrapezoidProfileCommand.h"
+
+#include <units/units.h>
+
+using namespace frc2;
+
+TrapezoidProfileCommand::TrapezoidProfileCommand(
+    frc::TrapezoidProfile profile,
+    std::function<void(frc::TrapezoidProfile::State)> output,
+    std::initializer_list<Subsystem*> requirements)
+    : m_profile(profile), m_output(output) {
+  AddRequirements(requirements);
+}
+
+void TrapezoidProfileCommand::Initialize() {
+  m_timer.Reset();
+  m_timer.Start();
+}
+
+void TrapezoidProfileCommand::Execute() {
+  m_output(m_profile.Calculate(m_timer.Get()));
+}
+
+void TrapezoidProfileCommand::End(bool interrupted) { m_timer.Stop(); }
+
+bool TrapezoidProfileCommand::IsFinished() {
+  return m_timer.HasPeriodPassed(m_profile.TotalTime());
+}

wpilibNewCommands/src/main/native/cpp/frc2/command/WaitCommand.cpp

@@ -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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/WaitCommand.h"
+
+using namespace frc2;
+
+WaitCommand::WaitCommand(units::second_t duration) : m_duration{duration} {
+  auto durationStr = std::to_string(duration.to<double>());
+  SetName(wpi::Twine(GetName()) + ": " + wpi::Twine(durationStr) + " seconds");
+}
+
+void WaitCommand::Initialize() {
+  m_timer.Reset();
+  m_timer.Start();
+}
+
+void WaitCommand::End(bool interrupted) { m_timer.Stop(); }
+
+bool WaitCommand::IsFinished() { return m_timer.HasPeriodPassed(m_duration); }
+
+bool WaitCommand::RunsWhenDisabled() const { return true; }

wpilibNewCommands/src/main/native/cpp/frc2/command/WaitUntilCommand.cpp

@@ -0,0 +1,20 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/WaitUntilCommand.h"
+
+using namespace frc2;
+
+WaitUntilCommand::WaitUntilCommand(std::function<bool()> condition)
+    : m_condition{std::move(condition)} {}
+
+WaitUntilCommand::WaitUntilCommand(double time)
+    : m_condition{[=] { return frc::Timer::GetMatchTime() - time > 0; }} {}
+
+bool WaitUntilCommand::IsFinished() { return m_condition(); }
+
+bool WaitUntilCommand::RunsWhenDisabled() const { return true; }

wpilibNewCommands/src/main/native/cpp/frc2/command/button/Button.cpp

@@ -0,0 +1,57 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/button/Button.h"
+
+using namespace frc2;
+
+Button::Button(std::function<bool()> isPressed) : Trigger(isPressed) {}
+
+Button Button::WhenPressed(Command* command, bool interruptible) {
+  WhenActive(command, interruptible);
+  return *this;
+}
+
+Button Button::WhenPressed(std::function<void()> toRun) {
+  WhenActive(std::move(toRun));
+  return *this;
+}
+
+Button Button::WhileHeld(Command* command, bool interruptible) {
+  WhileActiveContinous(command, interruptible);
+  return *this;
+}
+
+Button Button::WhileHeld(std::function<void()> toRun) {
+  WhileActiveContinous(std::move(toRun));
+  return *this;
+}
+
+Button Button::WhenHeld(Command* command, bool interruptible) {
+  WhileActiveOnce(command, interruptible);
+  return *this;
+}
+
+Button Button::WhenReleased(Command* command, bool interruptible) {
+  WhenInactive(command, interruptible);
+  return *this;
+}
+
+Button Button::WhenReleased(std::function<void()> toRun) {
+  WhenInactive(std::move(toRun));
+  return *this;
+}
+
+Button Button::ToggleWhenPressed(Command* command, bool interruptible) {
+  ToggleWhenActive(command, interruptible);
+  return *this;
+}
+
+Button Button::CancelWhenPressed(Command* command) {
+  CancelWhenActive(command);
+  return *this;
+}

wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp

@@ -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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/button/Trigger.h"
+
+#include <frc2/command/InstantCommand.h>
+
+using namespace frc2;
+
+Trigger::Trigger(const Trigger& other) : m_isActive(other.m_isActive) {}
+
+Trigger Trigger::WhenActive(Command* command, bool interruptible) {
+  CommandScheduler::GetInstance().AddButton(
+      [pressedLast = Get(), *this, command, interruptible]() mutable {
+        bool pressed = Get();
+
+        if (!pressedLast && pressed) {
+          command->Schedule(interruptible);
+        }
+
+        pressedLast = pressed;
+      });
+
+  return *this;
+}
+
+Trigger Trigger::WhenActive(std::function<void()> toRun) {
+  return WhenActive(InstantCommand(std::move(toRun), {}));
+}
+
+Trigger Trigger::WhileActiveContinous(Command* command, bool interruptible) {
+  CommandScheduler::GetInstance().AddButton(
+      [pressedLast = Get(), *this, command, interruptible]() mutable {
+        bool pressed = Get();
+
+        if (pressed) {
+          command->Schedule(interruptible);
+        } else if (pressedLast && !pressed) {
+          command->Cancel();
+        }
+
+        pressedLast = pressed;
+      });
+  return *this;
+}
+
+Trigger Trigger::WhileActiveContinous(std::function<void()> toRun) {
+  return WhileActiveContinous(InstantCommand(std::move(toRun), {}));
+}
+
+Trigger Trigger::WhileActiveOnce(Command* command, bool interruptible) {
+  CommandScheduler::GetInstance().AddButton(
+      [pressedLast = Get(), *this, command, interruptible]() mutable {
+        bool pressed = Get();
+
+        if (!pressedLast && pressed) {
+          command->Schedule(interruptible);
+        } else if (pressedLast && !pressed) {
+          command->Cancel();
+        }
+
+        pressedLast = pressed;
+      });
+  return *this;
+}
+
+Trigger Trigger::WhenInactive(Command* command, bool interruptible) {
+  CommandScheduler::GetInstance().AddButton(
+      [pressedLast = Get(), *this, command, interruptible]() mutable {
+        bool pressed = Get();
+
+        if (pressedLast && !pressed) {
+          command->Schedule(interruptible);
+        }
+
+        pressedLast = pressed;
+      });
+  return *this;
+}
+
+Trigger Trigger::WhenInactive(std::function<void()> toRun) {
+  return WhenInactive(InstantCommand(std::move(toRun), {}));
+}
+
+Trigger Trigger::ToggleWhenActive(Command* command, bool interruptible) {
+  CommandScheduler::GetInstance().AddButton(
+      [pressedLast = Get(), *this, command, interruptible]() mutable {
+        bool pressed = Get();
+
+        if (!pressedLast && pressed) {
+          if (command->IsScheduled()) {
+            command->Cancel();
+          } else {
+            command->Schedule(interruptible);
+          }
+        }
+
+        pressedLast = pressed;
+      });
+  return *this;
+}
+
+Trigger Trigger::CancelWhenActive(Command* command) {
+  CommandScheduler::GetInstance().AddButton(
+      [pressedLast = Get(), *this, command]() mutable {
+        bool pressed = Get();
+
+        if (!pressedLast && pressed) {
+          command->Cancel();
+        }
+
+        pressedLast = pressed;
+      });
+  return *this;
+}

wpilibNewCommands/src/main/native/include/frc2/command/Command.h

@@ -0,0 +1,242 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/ErrorBase.h>
+#include <frc/WPIErrors.h>
+#include <frc2/command/Subsystem.h>
+
+#include <memory>
+#include <string>
+
+#include <units/units.h>
+#include <wpi/ArrayRef.h>
+#include <wpi/Demangle.h>
+#include <wpi/SmallSet.h>
+#include <wpi/Twine.h>
+
+namespace frc2 {
+
+template <typename T>
+std::string GetTypeName(const T& type) {
+  return wpi::Demangle(typeid(type).name());
+}
+
+class ParallelCommandGroup;
+class ParallelRaceGroup;
+class ParallelDeadlineGroup;
+class SequentialCommandGroup;
+class PerpetualCommand;
+class ProxyScheduleCommand;
+
+/**
+ * A state machine representing a complete action to be performed by the robot.
+ * Commands are run by the 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.
+ *
+ * <p>Note: ALWAYS create a subclass by extending CommandHelper<Base, Subclass>,
+ * or decorators will not function!
+ *
+ * @see CommandScheduler
+ * @see CommandHelper
+ */
+class Command : public frc::ErrorBase {
+ public:
+  Command() = default;
+  virtual ~Command();
+
+  Command(const Command&);
+  Command& operator=(const Command&);
+  Command(Command&&) = default;
+  Command& operator=(Command&&) = default;
+
+  /**
+   * The initial subroutine of a command.  Called once when the command is
+   * initially scheduled.
+   */
+  virtual void Initialize();
+
+  /**
+   * The main body of a command.  Called repeatedly while the command is
+   * scheduled.
+   */
+  virtual 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
+   */
+  virtual void End(bool 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.
+   */
+  virtual bool 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
+   */
+  virtual wpi::SmallSet<Subsystem*, 4> GetRequirements() const = 0;
+
+  /**
+   * 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.
+   *
+   * @param duration the timeout duration
+   * @return the command with the timeout added
+   */
+  ParallelRaceGroup WithTimeout(units::second_t duration) &&;
+
+  /**
+   * 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.
+   *
+   * @param condition the interrupt condition
+   * @return the command with the interrupt condition added
+   */
+  ParallelRaceGroup WithInterrupt(std::function<bool()> condition) &&;
+
+  /**
+   * Decorates this command with a runnable to run before this command starts.
+   *
+   * @param toRun the Runnable to run
+   * @return the decorated command
+   */
+  SequentialCommandGroup BeforeStarting(std::function<void()> toRun) &&;
+
+  /**
+   * Decorates this command with a runnable to run after the command finishes.
+   *
+   * @param toRun the Runnable to run
+   * @return the decorated command
+   */
+  SequentialCommandGroup AndThen(std::function<void()> toRun) &&;
+
+  /**
+   * Decorates this command to run perpetually, ignoring its ordinary end
+   * conditions.  The decorated command can still be interrupted or canceled.
+   *
+   * @return the decorated command
+   */
+  PerpetualCommand Perpetually() &&;
+
+  /**
+   * 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
+   */
+  ProxyScheduleCommand AsProxy();
+
+  /**
+   * Schedules this command.
+   *
+   * @param interruptible whether this command can be interrupted by another
+   * command that shares one of its requirements
+   */
+  void Schedule(bool interruptible);
+
+  /**
+   * Schedules this command, defaulting to interruptible.
+   */
+  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.
+   */
+  void Cancel();
+
+  /**
+   * 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.
+   */
+  bool IsScheduled() const;
+
+  /**
+   * 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
+   */
+  bool HasRequirement(Subsystem* requirement) const;
+
+  /**
+   * Whether the command is currently grouped in a command group.  Used as extra
+   * insurance to prevent accidental independent use of grouped commands.
+   */
+  bool IsGrouped() const;
+
+  /**
+   * Sets whether the command is currently grouped in a command group.  Can be
+   * used to "reclaim" a command if a group is no longer going to use it.  NOT
+   * ADVISED!
+   */
+  void SetGrouped(bool grouped);
+
+  /**
+   * 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
+   */
+  virtual bool RunsWhenDisabled() const { return false; }
+
+  virtual std::string GetName() const;
+
+ protected:
+  /**
+   * Transfers ownership of this command to a unique pointer.  Used for
+   * decorator methods.
+   */
+  virtual std::unique_ptr<Command> TransferOwnership() && = 0;
+
+  bool m_isGrouped = false;
+};
+
+/**
+ * Checks if two commands have disjoint requirement sets.
+ *
+ * @param first The first command to check.
+ * @param second The second command to check.
+ * @return False if first and second share a requirement.
+ */
+bool RequirementsDisjoint(Command* first, Command* second);
+}  // namespace frc2

wpilibNewCommands/src/main/native/include/frc2/command/CommandBase.h

@@ -0,0 +1,73 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/smartdashboard/Sendable.h>
+#include <frc/smartdashboard/SendableHelper.h>
+
+#include <string>
+
+#include <wpi/SmallSet.h>
+#include <wpi/Twine.h>
+
+#include "Command.h"
+
+namespace frc2 {
+/**
+ * A Sendable base class for Commands.
+ */
+class CommandBase : public Command,
+                    public frc::Sendable,
+                    public frc::SendableHelper<CommandBase> {
+ public:
+  /**
+   * Adds the specified requirements to the command.
+   *
+   * @param requirements the requirements to add
+   */
+  void AddRequirements(std::initializer_list<Subsystem*> requirements);
+
+  void AddRequirements(wpi::SmallSet<Subsystem*, 4> requirements);
+
+  wpi::SmallSet<Subsystem*, 4> GetRequirements() const override;
+
+  /**
+   * Sets the name of this Command.
+   *
+   * @param name name
+   */
+  void SetName(const wpi::Twine& name);
+
+  /**
+   * Gets the name of this Command.
+   *
+   * @return Name
+   */
+  std::string GetName() const override;
+
+  /**
+   * Gets the subsystem name of this Command.
+   *
+   * @return Subsystem name
+   */
+  std::string GetSubsystem() const;
+
+  /**
+   * Sets the subsystem name of this Command.
+   *
+   * @param subsystem subsystem name
+   */
+  void SetSubsystem(const wpi::Twine& subsystem);
+
+  void InitSendable(frc::SendableBuilder& builder) override;
+
+ protected:
+  CommandBase();
+  wpi::SmallSet<Subsystem*, 4> m_requirements;
+};
+}  // namespace frc2

wpilibNewCommands/src/main/native/include/frc2/command/CommandGroupBase.h

@@ -0,0 +1,62 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/ErrorBase.h>
+
+#include <memory>
+#include <set>
+#include <vector>
+
+#include "CommandBase.h"
+
+namespace frc2 {
+
+/**
+ * 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.
+ */
+class CommandGroupBase : public CommandBase {
+ public:
+  /**
+   * Requires that the specified command not have been already allocated to a
+   * CommandGroup. Reports an error if the command is already grouped.
+   *
+   * @param commands The command to check
+   * @return True if all the command is ungrouped.
+   */
+  static bool RequireUngrouped(Command& command);
+
+  /**
+   * Requires that the specified commands not have been already allocated to a
+   * CommandGroup. Reports an error if any of the commands are already grouped.
+   *
+   * @param commands The commands to check
+   * @return True if all the commands are ungrouped.
+   */
+  static bool RequireUngrouped(wpi::ArrayRef<std::unique_ptr<Command>>);
+
+  /**
+   * Requires that the specified commands not have been already allocated to a
+   * CommandGroup. Reports an error if any of the commands are already grouped.
+   *
+   * @param commands The commands to check
+   * @return True if all the commands are ungrouped.
+   */
+  static bool RequireUngrouped(std::initializer_list<Command*>);
+
+  /**
+   * Adds the given commands to the command group.
+   *
+   * @param commands The commands to add.
+   */
+  virtual void AddCommands(
+      std::vector<std::unique_ptr<Command>>&& commands) = 0;
+};
+}  // namespace frc2

wpilibNewCommands/src/main/native/include/frc2/command/CommandHelper.h

@@ -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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+#include <type_traits>
+#include <utility>
+
+#include "Command.h"
+
+namespace frc2 {
+
+/**
+ * CRTP implementation to allow polymorphic decorator functions in Command.
+ *
+ * <p>Note: ALWAYS create a subclass by extending CommandHelper<Base, Subclass>,
+ * or decorators will not function!
+ */
+template <typename Base, typename CRTP,
+          typename = std::enable_if_t<std::is_base_of_v<Command, Base>>>
+class CommandHelper : public Base {
+  using Base::Base;
+
+ public:
+  CommandHelper() = default;
+
+ protected:
+  std::unique_ptr<Command> TransferOwnership() && override {
+    return std::make_unique<CRTP>(std::move(*static_cast<CRTP*>(this)));
+  }
+};
+}  // namespace frc2

wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h

@@ -0,0 +1,372 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/ErrorBase.h>
+#include <frc/RobotState.h>
+#include <frc/WPIErrors.h>
+#include <frc/smartdashboard/Sendable.h>
+#include <frc/smartdashboard/SendableHelper.h>
+
+#include <memory>
+#include <unordered_map>
+#include <utility>
+
+#include <networktables/NetworkTableEntry.h>
+#include <wpi/DenseMap.h>
+#include <wpi/FunctionExtras.h>
+#include <wpi/SmallSet.h>
+
+#include "CommandState.h"
+
+namespace frc2 {
+class Command;
+class Subsystem;
+
+/**
+ * The scheduler responsible for running Commands.  A Command-based robot should
+ * call 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 RegisterSubsystem() in order for their Periodic()
+ * methods to be called and for their default commands to be scheduled.
+ */
+class CommandScheduler final : public frc::Sendable,
+                               public frc::ErrorBase,
+                               public frc::SendableHelper<CommandScheduler> {
+ public:
+  /**
+   * Returns the Scheduler instance.
+   *
+   * @return the instance
+   */
+  static CommandScheduler& GetInstance();
+
+  using Action = std::function<void(const Command&)>;
+
+  /**
+   * Adds a button binding to the scheduler, which will be polled to schedule
+   * commands.
+   *
+   * @param button The button to add
+   */
+  void AddButton(wpi::unique_function<void()> button);
+
+  /**
+   * Removes all button bindings from the scheduler.
+   */
+  void ClearButtons();
+
+  /**
+   * 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
+   */
+  void Schedule(bool interruptible, Command* command);
+
+  /**
+   * Schedules a command for execution, with interruptible defaulted to true.
+   * Does nothing if the command is already scheduled.
+   *
+   * @param command the command to schedule
+   */
+  void Schedule(Command* command);
+
+  /**
+   * 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
+   */
+  void Schedule(bool interruptible, wpi::ArrayRef<Command*> commands);
+
+  /**
+   * 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
+   */
+  void Schedule(bool interruptible, std::initializer_list<Command*> commands);
+
+  /**
+   * Schedules multiple commands for execution, with interruptible defaulted to
+   * true.  Does nothing if the command is already scheduled.
+   *
+   * @param commands the commands to schedule
+   */
+  void Schedule(wpi::ArrayRef<Command*> commands);
+
+  /**
+   * Schedules multiple commands for execution, with interruptible defaulted to
+   * true.  Does nothing if the command is already scheduled.
+   *
+   * @param commands the commands to schedule
+   */
+  void Schedule(std::initializer_list<Command*> 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.
+   */
+  void Run();
+
+  /**
+   * 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 subsystem the subsystem to register
+   */
+  void RegisterSubsystem(Subsystem* subsystem);
+
+  /**
+   * 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 subsystem the subsystem to un-register
+   */
+  void UnregisterSubsystem(Subsystem* subsystem);
+
+  void RegisterSubsystem(std::initializer_list<Subsystem*> subsystems);
+
+  void UnregisterSubsystem(std::initializer_list<Subsystem*> 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 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
+   */
+  template <class T, typename = std::enable_if_t<std::is_base_of_v<
+                         Command, std::remove_reference_t<T>>>>
+  void SetDefaultCommand(Subsystem* subsystem, T&& defaultCommand) {
+    if (!defaultCommand.HasRequirement(subsystem)) {
+      wpi_setWPIErrorWithContext(
+          CommandIllegalUse, "Default commands must require their subsystem!");
+      return;
+    }
+    if (defaultCommand.IsFinished()) {
+      wpi_setWPIErrorWithContext(CommandIllegalUse,
+                                 "Default commands should not end!");
+      return;
+    }
+    m_subsystems[subsystem] = std::make_unique<std::remove_reference_t<T>>(
+        std::forward<T>(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
+   */
+  Command* GetDefaultCommand(const Subsystem* subsystem) const;
+
+  /**
+   * Cancels a command.  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 command the command to cancel
+   */
+  void Cancel(Command* command);
+
+  /**
+   * 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
+   */
+  void Cancel(wpi::ArrayRef<Command*> commands);
+
+  /**
+   * 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
+   */
+  void Cancel(std::initializer_list<Command*> commands);
+
+  /**
+   * Cancels all commands that are currently scheduled.
+   */
+  void CancelAll();
+
+  /**
+   * 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
+   */
+  double TimeSinceScheduled(const Command* command) const;
+
+  /**
+   * 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
+   */
+  bool IsScheduled(wpi::ArrayRef<const Command*> commands) const;
+
+  /**
+   * 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
+   */
+  bool IsScheduled(std::initializer_list<const Command*> commands) const;
+
+  /**
+   * Whether a given command is 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
+   */
+  bool IsScheduled(const Command* command) const;
+
+  /**
+   * 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
+   */
+  Command* Requiring(const Subsystem* subsystem) const;
+
+  /**
+   * Disables the command scheduler.
+   */
+  void Disable();
+
+  /**
+   * Enables the command scheduler.
+   */
+  void Enable();
+
+  /**
+   * Adds an action to perform on the initialization of any command by the
+   * scheduler.
+   *
+   * @param action the action to perform
+   */
+  void OnCommandInitialize(Action action);
+
+  /**
+   * Adds an action to perform on the execution of any command by the scheduler.
+   *
+   * @param action the action to perform
+   */
+  void OnCommandExecute(Action action);
+
+  /**
+   * Adds an action to perform on the interruption of any command by the
+   * scheduler.
+   *
+   * @param action the action to perform
+   */
+  void OnCommandInterrupt(Action action);
+
+  /**
+   * Adds an action to perform on the finishing of any command by the scheduler.
+   *
+   * @param action the action to perform
+   */
+  void OnCommandFinish(Action action);
+
+  void InitSendable(frc::SendableBuilder& builder) override;
+
+ private:
+  // Constructor; private as this is a singleton
+  CommandScheduler();
+
+  // A map from commands to their scheduling state.  Also used as a set of the
+  // currently-running commands.
+  wpi::DenseMap<Command*, CommandState> m_scheduledCommands;
+
+  // A map from required subsystems to their requiring commands.  Also used as a
+  // set of the currently-required subsystems.
+  wpi::DenseMap<Subsystem*, Command*> m_requirements;
+
+  // A map from subsystems registered with the scheduler to their default
+  // commands.  Also used as a list of currently-registered subsystems.
+  wpi::DenseMap<Subsystem*, std::unique_ptr<Command>> m_subsystems;
+
+  // The set of currently-registered buttons that will be polled every
+  // iteration.
+  wpi::SmallVector<wpi::unique_function<void()>, 4> m_buttons;
+
+  bool m_disabled{false};
+
+  // NetworkTable entries for use in Sendable impl
+  nt::NetworkTableEntry m_namesEntry;
+  nt::NetworkTableEntry m_idsEntry;
+  nt::NetworkTableEntry m_cancelEntry;
+
+  // Lists of user-supplied actions to be executed on scheduling events for
+  // every command.
+  wpi::SmallVector<Action, 4> m_initActions;
+  wpi::SmallVector<Action, 4> m_executeActions;
+  wpi::SmallVector<Action, 4> m_interruptActions;
+  wpi::SmallVector<Action, 4> m_finishActions;
+
+  // Flag and queues for avoiding concurrent modification if commands are
+  // scheduled/canceled during run
+
+  bool m_inRunLoop = false;
+  wpi::DenseMap<Command*, bool> m_toSchedule;
+  wpi::SmallVector<Command*, 4> m_toCancel;
+
+  friend class CommandTestBase;
+};
+}  // namespace frc2

wpilibNewCommands/src/main/native/include/frc2/command/CommandState.h

@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+namespace frc2 {
+/**
+ * Class that holds scheduling state for a command.  Used internally by the
+ * CommandScheduler
+ */
+class CommandState final {
+ public:
+  CommandState() = default;
+
+  explicit CommandState(bool interruptible);
+
+  bool IsInterruptible() const { return m_interruptible; }
+
+  // The time since this command was initialized.
+  double TimeSinceInitialized() const;
+
+ private:
+  double m_startTime = -1;
+  bool m_interruptible;
+
+  void StartTiming();
+  void StartRunning();
+};
+}  // namespace frc2

wpilibNewCommands/src/main/native/include/frc2/command/ConditionalCommand.h

@@ -0,0 +1,92 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <iostream>
+#include <memory>
+#include <utility>
+
+#include "CommandBase.h"
+#include "CommandGroupBase.h"
+#include "CommandHelper.h"
+
+namespace frc2 {
+/**
+ * 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
+ * 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.
+ *
+ * @see ScheduleCommand
+ */
+class ConditionalCommand
+    : public CommandHelper<CommandBase, ConditionalCommand> {
+ public:
+  /**
+   * 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
+   */
+  template <class T1, class T2,
+            typename = std::enable_if_t<
+                std::is_base_of_v<Command, std::remove_reference_t<T1>>>,
+            typename = std::enable_if_t<
+                std::is_base_of_v<Command, std::remove_reference_t<T2>>>>
+  ConditionalCommand(T1&& onTrue, T2&& onFalse, std::function<bool()> condition)
+      : ConditionalCommand(std::make_unique<std::remove_reference_t<T1>>(
+                               std::forward<T1>(onTrue)),
+                           std::make_unique<std::remove_reference_t<T2>>(
+                               std::forward<T2>(onFalse)),
+                           condition) {}
+
+  /**
+   * 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
+   */
+  ConditionalCommand(std::unique_ptr<Command>&& onTrue,
+                     std::unique_ptr<Command>&& onFalse,
+                     std::function<bool()> condition);
+
+  ConditionalCommand(ConditionalCommand&& other) = default;
+
+  // No copy constructors for command groups
+  ConditionalCommand(const ConditionalCommand& other) = delete;
+
+  void Initialize() override;
+
+  void Execute() override;
+
+  void End(bool interrupted) override;
+
+  bool IsFinished() override;
+
+  bool RunsWhenDisabled() const override;
+
+ private:
+  std::unique_ptr<Command> m_onTrue;
+  std::unique_ptr<Command> m_onFalse;
+  std::function<bool()> m_condition;
+  Command* m_selectedCommand{nullptr};
+  bool m_runsWhenDisabled = true;
+};
+}  // namespace frc2

wpilibNewCommands/src/main/native/include/frc2/command/FunctionalCommand.h

@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "CommandBase.h"
+#include "CommandHelper.h"
+
+namespace frc2 {
+/**
+ * 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.
+ */
+class FunctionalCommand : public CommandHelper<CommandBase, FunctionalCommand> {
+ public:
+  /**
+   * 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
+   */
+  FunctionalCommand(std::function<void()> onInit,
+                    std::function<void()> onExecute,
+                    std::function<void(bool)> onEnd,
+                    std::function<bool()> isFinished);
+
+  FunctionalCommand(FunctionalCommand&& other) = default;
+
+  FunctionalCommand(const FunctionalCommand& other) = default;
+
+  void Initialize() override;
+
+  void Execute() override;
+
+  void End(bool interrupted) override;
+
+  bool IsFinished() override;
+
+ private:
+  std::function<void()> m_onInit;
+  std::function<void()> m_onExecute;
+  std::function<void(bool)> m_onEnd;
+  std::function<bool()> m_isFinished;
+};
+}  // namespace frc2

wpilibNewCommands/src/main/native/include/frc2/command/InstantCommand.h

@@ -0,0 +1,48 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "CommandBase.h"
+#include "CommandHelper.h"
+
+namespace frc2 {
+/**
+ * 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.
+ */
+class InstantCommand : public CommandHelper<CommandBase, InstantCommand> {
+ public:
+  /**
+   * 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
+   */
+  InstantCommand(std::function<void()> toRun,
+                 std::initializer_list<Subsystem*> requirements);
+
+  InstantCommand(InstantCommand&& other) = default;
+
+  InstantCommand(const InstantCommand& other) = default;
+
+  /**
+   * Creates a new InstantCommand with a Runnable that does nothing.  Useful
+   * only as a no-arg constructor to call implicitly from subclass constructors.
+   */
+  InstantCommand();
+
+  void Initialize() override;
+
+  bool IsFinished() final;
+
+ private:
+  std::function<void()> m_toRun;
+};
+}  // namespace frc2

wpilibNewCommands/src/main/native/include/frc2/command/NotifierCommand.h

@@ -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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/Notifier.h>
+
+#include <units/units.h>
+
+#include "CommandBase.h"
+#include "CommandHelper.h"
+
+namespace frc2 {
+/**
+ * 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#withInterrupt(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.
+ */
+class NotifierCommand : public CommandHelper<CommandBase, NotifierCommand> {
+ public:
+  /**
+   * Creates a new NotifierCommand.
+   *
+   * @param toRun        the runnable for the notifier to run
+   * @param period       the period at which the notifier should run
+   * @param requirements the subsystems required by this command
+   */
+  NotifierCommand(std::function<void()> toRun, units::second_t period,
+                  std::initializer_list<Subsystem*> requirements);
+
+  NotifierCommand(NotifierCommand&& other);
+
+  NotifierCommand(const NotifierCommand& other);
+
+  void Initialize() override;
+
+  void End(bool interrupted) override;
+
+ private:
+  std::function<void()> m_toRun;
+  frc::Notifier m_notifier;
+  units::second_t m_period;
+};
+}  // namespace frc2

wpilibNewCommands/src/main/native/include/frc2/command/PIDCommand.h

@@ -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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "frc/controller/PIDController.h"
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.h"
+
+namespace frc2 {
+/**
+ * A command that controls an output with a 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.
+ *
+ * @see PIDController
+ */
+class PIDCommand : public CommandHelper<CommandBase, PIDCommand> {
+ public:
+  /**
+   * 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 reference (aka setpoint)
+   * @param useOutput         the controller's output
+   * @param requirements      the subsystems required by this command
+   */
+  PIDCommand(PIDController controller,
+             std::function<double()> measurementSource,
+             std::function<double()> setpointSource,
+             std::function<void(double)> useOutput,
+             std::initializer_list<Subsystem*> requirements);
+
+  /**
+   * Creates a new PIDCommand, which controls the given output with a
+   * PIDController with a constant setpoint.
+   *
+   * @param controller        the controller that controls the output.
+   * @param measurementSource the measurement of the process variable
+   * @param setpoint         the controller's setpoint (aka setpoint)
+   * @param useOutput         the controller's output
+   * @param requirements      the subsystems required by this command
+   */
+  PIDCommand(PIDController controller,
+             std::function<double()> measurementSource, double setpoint,
+             std::function<void(double)> useOutput,
+             std::initializer_list<Subsystem*> requirements);
+
+  PIDCommand(PIDCommand&& other) = default;
+
+  PIDCommand(const PIDCommand& other) = default;
+
+  void Initialize() override;
+
+  void Execute() override;
+
+  void End(bool interrupted) override;
+
+  /**
+   * Returns the PIDController used by the command.
+   *
+   * @return The PIDController
+   */
+  PIDController& getController();
+
+ protected:
+  PIDController m_controller;
+  std::function<double()> m_measurement;
+  std::function<double()> m_setpoint;
+  std::function<void(double)> m_useOutput;
+};
+}  // namespace frc2

wpilibNewCommands/src/main/native/include/frc2/command/PIDSubsystem.h

@@ -0,0 +1,73 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "frc/controller/PIDController.h"
+#include "frc2/command/SubsystemBase.h"
+
+namespace frc2 {
+/**
+ * A subsystem that uses a PIDController to control an output.  The controller
+ * is run synchronously from the subsystem's periodic() method.
+ *
+ * @see PIDController
+ */
+class PIDSubsystem : public SubsystemBase {
+ public:
+  /**
+   * Creates a new PIDSubsystem.
+   *
+   * @param controller the PIDController to use
+   */
+  explicit PIDSubsystem(PIDController controller);
+
+  void Periodic() override;
+
+  /**
+   * Uses the output from the PIDController.
+   *
+   * @param output the output of the PIDController
+   */
+  virtual void UseOutput(double output) = 0;
+
+  /**
+   * Returns the reference (setpoint) used by the PIDController.
+   *
+   * @return the reference (setpoint) to be used by the controller
+   */
+  virtual double GetSetpoint() = 0;
+
+  /**
+   * Returns the measurement of the process variable used by the PIDController.
+   *
+   * @return the measurement of the process variable
+   */
+  virtual double GetMeasurement() = 0;
+
+  /**
+   * Enables the PID control.  Resets the controller.
+   */
+  virtual void Enable();
+
+  /**
+   * Disables the PID control.  Sets output to zero.
+   */
+  virtual void Disable();
+
+  /**
+   * Returns the PIDController.
+   *
+   * @return The controller.
+   */
+  PIDController& GetController();
+
+ protected:
+  PIDController m_controller;
+  bool m_enabled;
+};
+}  // namespace frc2

wpilibNewCommands/src/main/native/include/frc2/command/ParallelCommandGroup.h

@@ -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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#ifdef _WIN32
+#pragma warning(push)
+#pragma warning(disable : 4521)
+#endif
+
+#include <memory>
+#include <unordered_map>
+#include <utility>
+#include <vector>
+
+#include "CommandGroupBase.h"
+#include "CommandHelper.h"
+
+namespace frc2 {
+/**
+ * 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.
+ */
+class ParallelCommandGroup
+    : public CommandHelper<CommandGroupBase, ParallelCommandGroup> {
+ public:
+  /**
+   * 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.
+   */
+  explicit ParallelCommandGroup(
+      std::vector<std::unique_ptr<Command>>&& commands);
+
+  /**
+   * 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.
+   */
+  template <class... Types,
+            typename = std::enable_if_t<std::conjunction_v<
+                std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
+  explicit ParallelCommandGroup(Types&&... commands) {
+    AddCommands(std::forward<Types>(commands)...);
+  }
+
+  ParallelCommandGroup(ParallelCommandGroup&& other) = default;
+
+  // No copy constructors for commandgroups
+  ParallelCommandGroup(const ParallelCommandGroup&) = delete;
+
+  // Prevent template expansion from emulating copy ctor
+  ParallelCommandGroup(ParallelCommandGroup&) = delete;
+
+  template <class... Types,
+            typename = std::enable_if_t<std::conjunction_v<
+                std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
+  void AddCommands(Types&&... commands) {
+    std::vector<std::unique_ptr<Command>> foo;
+    ((void)foo.emplace_back(std::make_unique<std::remove_reference_t<Types>>(
+         std::forward<Types>(commands))),
+     ...);
+    AddCommands(std::move(foo));
+  }
+
+  void Initialize() override;
+
+  void Execute() override;
+
+  void End(bool interrupted) override;
+
+  bool IsFinished() override;
+
+  bool RunsWhenDisabled() const override;
+
+ private:
+  void AddCommands(std::vector<std::unique_ptr<Command>>&& commands) override;
+
+  std::unordered_map<std::unique_ptr<Command>, bool> m_commands;
+  bool m_runWhenDisabled{true};
+  bool isRunning = false;
+};
+}  // namespace frc2
+
+#ifdef _WIN32
+#pragma warning(pop)
+#endif

wpilibNewCommands/src/main/native/include/frc2/command/ParallelDeadlineGroup.h

@@ -0,0 +1,111 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#ifdef _WIN32
+#pragma warning(push)
+#pragma warning(disable : 4521)
+#endif
+
+#include <memory>
+#include <unordered_map>
+#include <utility>
+#include <vector>
+
+#include "CommandGroupBase.h"
+#include "CommandHelper.h"
+
+namespace frc2 {
+/**
+ * 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.
+ */
+class ParallelDeadlineGroup
+    : public CommandHelper<CommandGroupBase, ParallelDeadlineGroup> {
+ public:
+  /**
+   * 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
+   */
+  ParallelDeadlineGroup(std::unique_ptr<Command>&& deadline,
+                        std::vector<std::unique_ptr<Command>>&& commands);
+  /**
+   * 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
+   */
+  template <class T, class... Types,
+            typename = std::enable_if_t<
+                std::is_base_of_v<Command, std::remove_reference_t<T>>>,
+            typename = std::enable_if_t<std::conjunction_v<
+                std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
+  explicit ParallelDeadlineGroup(T&& deadline, Types&&... commands) {
+    SetDeadline(std::make_unique<std::remove_reference_t<T>>(
+        std::forward<T>(deadline)));
+    AddCommands(std::forward<Types>(commands)...);
+  }
+
+  ParallelDeadlineGroup(ParallelDeadlineGroup&& other) = default;
+
+  // No copy constructors for command groups
+  ParallelDeadlineGroup(const ParallelDeadlineGroup&) = delete;
+
+  // Prevent template expansion from emulating copy ctor
+  ParallelDeadlineGroup(ParallelDeadlineGroup&) = delete;
+
+  template <class... Types,
+            typename = std::enable_if_t<std::conjunction_v<
+                std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
+  void AddCommands(Types&&... commands) {
+    std::vector<std::unique_ptr<Command>> foo;
+    ((void)foo.emplace_back(std::make_unique<std::remove_reference_t<Types>>(
+         std::forward<Types>(commands))),
+     ...);
+    AddCommands(std::move(foo));
+  }
+
+  void Initialize() override;
+
+  void Execute() override;
+
+  void End(bool interrupted) override;
+
+  bool IsFinished() override;
+
+  bool RunsWhenDisabled() const override;
+
+ private:
+  void AddCommands(std::vector<std::unique_ptr<Command>>&& commands) override;
+
+  void SetDeadline(std::unique_ptr<Command>&& deadline);
+
+  std::unordered_map<std::unique_ptr<Command>, bool> m_commands;
+  Command* m_deadline;
+  bool m_runWhenDisabled{true};
+  bool isRunning = false;
+};
+}  // namespace frc2
+
+#ifdef _WIN32
+#pragma warning(pop)
+#endif

wpilibNewCommands/src/main/native/include/frc2/command/ParallelRaceGroup.h

@@ -0,0 +1,90 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#ifdef _WIN32
+#pragma warning(push)
+#pragma warning(disable : 4521)
+#endif
+
+#include <memory>
+#include <set>
+#include <unordered_map>
+#include <utility>
+#include <vector>
+
+#include "CommandGroupBase.h"
+#include "CommandHelper.h"
+
+namespace frc2 {
+/**
+ * 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.
+ */
+class ParallelRaceGroup
+    : public CommandHelper<CommandGroupBase, ParallelRaceGroup> {
+ public:
+  /**
+   * 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.
+   */
+  explicit ParallelRaceGroup(std::vector<std::unique_ptr<Command>>&& commands);
+
+  template <class... Types,
+            typename = std::enable_if_t<std::conjunction_v<
+                std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
+  explicit ParallelRaceGroup(Types&&... commands) {
+    AddCommands(std::forward<Types>(commands)...);
+  }
+
+  ParallelRaceGroup(ParallelRaceGroup&& other) = default;
+
+  // No copy constructors for command groups
+  ParallelRaceGroup(const ParallelRaceGroup&) = delete;
+
+  // Prevent template expansion from emulating copy ctor
+  ParallelRaceGroup(ParallelRaceGroup&) = delete;
+
+  template <class... Types>
+  void AddCommands(Types&&... commands) {
+    std::vector<std::unique_ptr<Command>> foo;
+    ((void)foo.emplace_back(std::make_unique<std::remove_reference_t<Types>>(
+         std::forward<Types>(commands))),
+     ...);
+    AddCommands(std::move(foo));
+  }
+
+  void Initialize() override;
+
+  void Execute() override;
+
+  void End(bool interrupted) override;
+
+  bool IsFinished() override;
+
+  bool RunsWhenDisabled() const override;
+
+ private:
+  void AddCommands(std::vector<std::unique_ptr<Command>>&& commands) override;
+
+  std::set<std::unique_ptr<Command>> m_commands;
+  bool m_runWhenDisabled{true};
+  bool m_finished{false};
+  bool isRunning = false;
+};
+}  // namespace frc2
+
+#ifdef _WIN32
+#pragma warning(pop)
+#endif

wpilibNewCommands/src/main/native/include/frc2/command/PerpetualCommand.h

@@ -0,0 +1,78 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#ifdef _WIN32
+#pragma warning(push)
+#pragma warning(disable : 4521)
+#endif
+
+#include <memory>
+#include <utility>
+
+#include "CommandBase.h"
+#include "CommandGroupBase.h"
+#include "CommandHelper.h"
+
+namespace frc2 {
+/**
+ * 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.
+ */
+class PerpetualCommand : public CommandHelper<CommandBase, PerpetualCommand> {
+ public:
+  /**
+   * 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
+   */
+  explicit PerpetualCommand(std::unique_ptr<Command>&& 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
+   */
+  template <class T, typename = std::enable_if_t<std::is_base_of_v<
+                         Command, std::remove_reference_t<T>>>>
+  explicit PerpetualCommand(T&& command)
+      : PerpetualCommand(std::make_unique<std::remove_reference_t<T>>(
+            std::forward<T>(command))) {}
+
+  PerpetualCommand(PerpetualCommand&& other) = default;
+
+  // No copy constructors for command groups
+  PerpetualCommand(const PerpetualCommand& other) = delete;
+
+  // Prevent template expansion from emulating copy ctor
+  PerpetualCommand(PerpetualCommand&) = delete;
+
+  void Initialize() override;
+
+  void Execute() override;
+
+  void End(bool interrupted) override;
+
+ private:
+  std::unique_ptr<Command> m_command;
+};
+}  // namespace frc2
+
+#ifdef _WIN32
+#pragma warning(pop)
+#endif

wpilibNewCommands/src/main/native/include/frc2/command/PrintCommand.h

@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <wpi/Twine.h>
+#include <wpi/raw_ostream.h>
+
+#include "CommandHelper.h"
+#include "InstantCommand.h"
+
+namespace frc2 {
+/**
+ * A command that prints a string when initialized.
+ */
+class PrintCommand : public CommandHelper<InstantCommand, PrintCommand> {
+ public:
+  /**
+   * Creates a new a PrintCommand.
+   *
+   * @param message the message to print
+   */
+  explicit PrintCommand(const wpi::Twine& message);
+
+  PrintCommand(PrintCommand&& other) = default;
+
+  PrintCommand(const PrintCommand& other) = default;
+
+  bool RunsWhenDisabled() const override;
+};
+}  // namespace frc2

wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h

@@ -0,0 +1,116 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <units/units.h>
+
+#include "frc/controller/ProfiledPIDController.h"
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.h"
+
+namespace frc2 {
+/**
+ * A command that controls an output with a ProfiledPIDController.  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.
+ *
+ * @see ProfiledPIDController
+ */
+class ProfiledPIDCommand
+    : public CommandHelper<CommandBase, ProfiledPIDCommand> {
+  using State = frc::TrapezoidProfile::State;
+
+ public:
+  /**
+   * Creates a new PIDCommand, which controls the given output with a
+   * ProfiledPIDController.
+   *
+   * @param controller        the controller that controls the output.
+   * @param measurementSource the measurement of the process variable
+   * @param goalSource   the controller's goal
+   * @param useOutput         the controller's output
+   * @param requirements      the subsystems required by this command
+   */
+  ProfiledPIDCommand(frc::ProfiledPIDController controller,
+                     std::function<units::meter_t()> measurementSource,
+                     std::function<State()> goalSource,
+                     std::function<void(double, State)> useOutput,
+                     std::initializer_list<Subsystem*> requirements);
+
+  /**
+   * Creates a new PIDCommand, which controls the given output with a
+   * ProfiledPIDController.
+   *
+   * @param controller        the controller that controls the output.
+   * @param measurementSource the measurement of the process variable
+   * @param goalSource   the controller's goal
+   * @param useOutput         the controller's output
+   * @param requirements      the subsystems required by this command
+   */
+  ProfiledPIDCommand(frc::ProfiledPIDController controller,
+                     std::function<units::meter_t()> measurementSource,
+                     std::function<units::meter_t()> goalSource,
+                     std::function<void(double, State)> useOutput,
+                     std::initializer_list<Subsystem*> requirements);
+
+  /**
+   * Creates a new PIDCommand, which controls the given output with a
+   * ProfiledPIDController with a constant goal.
+   *
+   * @param controller        the controller that controls the output.
+   * @param measurementSource the measurement of the process variable
+   * @param goal         the controller's goal
+   * @param useOutput         the controller's output
+   * @param requirements      the subsystems required by this command
+   */
+  ProfiledPIDCommand(frc::ProfiledPIDController controller,
+                     std::function<units::meter_t()> measurementSource,
+                     State goal, std::function<void(double, State)> useOutput,
+                     std::initializer_list<Subsystem*> requirements);
+
+  /**
+   * Creates a new PIDCommand, which controls the given output with a
+   * ProfiledPIDController with a constant goal.
+   *
+   * @param controller        the controller that controls the output.
+   * @param measurementSource the measurement of the process variable
+   * @param goal         the controller's goal
+   * @param useOutput         the controller's output
+   * @param requirements      the subsystems required by this command
+   */
+  ProfiledPIDCommand(frc::ProfiledPIDController controller,
+                     std::function<units::meter_t()> measurementSource,
+                     units::meter_t goal,
+                     std::function<void(double, State)> useOutput,
+                     std::initializer_list<Subsystem*> requirements);
+
+  ProfiledPIDCommand(ProfiledPIDCommand&& other) = default;
+
+  ProfiledPIDCommand(const ProfiledPIDCommand& other) = default;
+
+  void Initialize() override;
+
+  void Execute() override;
+
+  void End(bool interrupted) override;
+
+  /**
+   * Returns the ProfiledPIDController used by the command.
+   *
+   * @return The ProfiledPIDController
+   */
+  frc::ProfiledPIDController& GetController();
+
+ protected:
+  frc::ProfiledPIDController m_controller;
+  std::function<units::meter_t()> m_measurement;
+  std::function<State()> m_goal;
+  std::function<void(double, State)> m_useOutput;
+};
+}  // namespace frc2

wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h

@@ -0,0 +1,78 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <units/units.h>
+
+#include "frc/controller/ProfiledPIDController.h"
+#include "frc2/command/SubsystemBase.h"
+
+namespace frc2 {
+/**
+ * A subsystem that uses a ProfiledPIDController to control an output.  The
+ * controller is run synchronously from the subsystem's periodic() method.
+ *
+ * @see ProfiledPIDController
+ */
+class ProfiledPIDSubsystem : public SubsystemBase {
+  using State = frc::TrapezoidProfile::State;
+
+ public:
+  /**
+   * Creates a new ProfiledPIDSubsystem.
+   *
+   * @param controller the ProfiledPIDController to use
+   */
+  explicit ProfiledPIDSubsystem(frc::ProfiledPIDController controller);
+
+  void Periodic() override;
+
+  /**
+   * Uses the output from the ProfiledPIDController.
+   *
+   * @param output the output of the ProfiledPIDController
+   */
+  virtual void UseOutput(double output, State state) = 0;
+
+  /**
+   * Returns the goal used by the ProfiledPIDController.
+   *
+   * @return the goal to be used by the controller
+   */
+  virtual State GetGoal() = 0;
+
+  /**
+   * Returns the measurement of the process variable used by the
+   * ProfiledPIDController.
+   *
+   * @return the measurement of the process variable
+   */
+  virtual units::meter_t GetMeasurement() = 0;
+
+  /**
+   * Enables the PID control.  Resets the controller.
+   */
+  virtual void Enable();
+
+  /**
+   * Disables the PID control.  Sets output to zero.
+   */
+  virtual void Disable();
+
+  /**
+   * Returns the ProfiledPIDController.
+   *
+   * @return The controller.
+   */
+  frc::ProfiledPIDController& GetController();
+
+ protected:
+  frc::ProfiledPIDController m_controller;
+  bool m_enabled;
+};
+}  // namespace frc2

wpilibNewCommands/src/main/native/include/frc2/command/ProxyScheduleCommand.h

@@ -0,0 +1,50 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <wpi/SmallVector.h>
+
+#include "CommandBase.h"
+#include "CommandHelper.h"
+#include "SetUtilities.h"
+
+namespace frc2 {
+/**
+ * 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.
+ */
+class ProxyScheduleCommand
+    : public CommandHelper<CommandBase, ProxyScheduleCommand> {
+ public:
+  /**
+   * 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
+   */
+  explicit ProxyScheduleCommand(wpi::ArrayRef<Command*> toSchedule);
+
+  ProxyScheduleCommand(ProxyScheduleCommand&& other) = default;
+
+  ProxyScheduleCommand(const ProxyScheduleCommand& other) = default;
+
+  void Initialize() override;
+
+  void End(bool interrupted) override;
+
+  void Execute() override;
+
+  bool IsFinished() override;
+
+ private:
+  wpi::SmallVector<Command*, 4> m_toSchedule;
+  bool m_finished{false};
+};
+}  // namespace frc2

wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h

@@ -0,0 +1,148 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <functional>
+#include <memory>
+
+#include <units/units.h>
+
+#include "CommandBase.h"
+#include "CommandHelper.h"
+#include "frc/controller/PIDController.h"
+#include "frc/controller/RamseteController.h"
+#include "frc/geometry/Pose2d.h"
+#include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "frc/trajectory/Trajectory.h"
+#include "frc2/Timer.h"
+
+#pragma once
+
+namespace frc2 {
+/**
+ * A command that uses a RAMSETE controller  to follow a trajectory
+ * with a differential drive.
+ *
+ * <p>The command handles trajectory-following, PID calculations, and
+ * feedforwards internally.  This is intended to be a more-or-less "complete
+ * solution" that can be used by teams without a great deal of controls
+ * expertise.
+ *
+ * <p>Advanced teams seeking more flexibility (for example, those who wish to
+ * use the onboard PID functionality of a "smart" motor controller) may use the
+ * secondary constructor that omits the PID and feedforward functionality,
+ * returning only the raw wheel speeds from the RAMSETE controller.
+ *
+ * @see RamseteController
+ * @see Trajectory
+ */
+class RamseteCommand : public CommandHelper<CommandBase, RamseteCommand> {
+  using voltsecondspermeter =
+      units::compound_unit<units::volt, units::second,
+                           units::inverse<units::meter>>;
+  using voltsecondssquaredpermeter =
+      units::compound_unit<units::volt, units::squared<units::second>,
+                           units::inverse<units::meter>>;
+
+ public:
+  /**
+   * Constructs a new RamseteCommand that, when executed, will follow the
+   * provided trajectory. PID control and feedforward are handled internally,
+   * and outputs are scaled -1 to 1 for easy consumption by speed controllers.
+   *
+   * <p>Note: The controller will *not* set the outputVolts to zero upon
+   * completion of the path - this is left to the user, since it is not
+   * appropriate for paths with nonstationary endstates.
+   *
+   * @param trajectory      The trajectory to follow.
+   * @param pose            A function that supplies the robot pose - use one of
+   * the odometry classes to provide this.
+   * @param controller      The RAMSETE controller used to follow the
+   * trajectory.
+   * @param ks              Constant feedforward term for the robot drive.
+   * @param kv              Velocity-proportional feedforward term for the robot
+   * drive.
+   * @param ka              Acceleration-proportional feedforward term for the
+   * robot drive.
+   * @param kinematics      The kinematics for the robot drivetrain.
+   * @param leftSpeed       A function that supplies the speed of the left side
+   * of the robot drive.
+   * @param rightSpeed      A function that supplies the speed of the right side
+   * of the robot drive.
+   * @param leftController  The PIDController for the left side of the robot
+   * drive.
+   * @param rightController The PIDController for the right side of the robot
+   * drive.
+   * @param output          A function that consumes the computed left and right
+   * outputs (in volts) for the robot drive.
+   * @param requirements    The subsystems to require.
+   */
+  RamseteCommand(frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+                 frc::RamseteController controller, units::volt_t ks,
+                 units::unit_t<voltsecondspermeter> kv,
+                 units::unit_t<voltsecondssquaredpermeter> ka,
+                 frc::DifferentialDriveKinematics kinematics,
+                 std::function<units::meters_per_second_t()> leftSpeed,
+                 std::function<units::meters_per_second_t()> rightSpeed,
+                 frc2::PIDController leftController,
+                 frc2::PIDController rightController,
+                 std::function<void(units::volt_t, units::volt_t)> output,
+                 std::initializer_list<Subsystem*> requirements);
+
+  /**
+   * Constructs a new RamseteCommand that, when executed, will follow the
+   * provided trajectory. Performs no PID control and calculates no
+   * feedforwards; outputs are the raw wheel speeds from the RAMSETE controller,
+   * and will need to be converted into a usable form by the user.
+   *
+   * @param trajectory      The trajectory to follow.
+   * @param pose            A function that supplies the robot pose - use one of
+   * the odometry classes to provide this.
+   * @param controller      The RAMSETE controller used to follow the
+   * trajectory.
+   * @param kinematics      The kinematics for the robot drivetrain.
+   * @param output          A function that consumes the computed left and right
+   * outputs (in volts) for the robot drive.
+   * @param requirements    The subsystems to require.
+   */
+  RamseteCommand(frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+                 frc::RamseteController controller,
+                 frc::DifferentialDriveKinematics kinematics,
+                 std::function<void(units::meters_per_second_t,
+                                    units::meters_per_second_t)>
+                     output,
+                 std::initializer_list<Subsystem*> requirements);
+
+  void Initialize() override;
+
+  void Execute() override;
+
+  void End(bool interrupted) override;
+
+  bool IsFinished() override;
+
+ private:
+  frc::Trajectory m_trajectory;
+  std::function<frc::Pose2d()> m_pose;
+  frc::RamseteController m_controller;
+  const units::volt_t m_ks;
+  const units::unit_t<voltsecondspermeter> m_kv;
+  const units::unit_t<voltsecondssquaredpermeter> m_ka;
+  frc::DifferentialDriveKinematics m_kinematics;
+  std::function<units::meters_per_second_t()> m_leftSpeed;
+  std::function<units::meters_per_second_t()> m_rightSpeed;
+  std::unique_ptr<frc2::PIDController> m_leftController;
+  std::unique_ptr<frc2::PIDController> m_rightController;
+  std::function<void(units::volt_t, units::volt_t)> m_outputVolts;
+  std::function<void(units::meters_per_second_t, units::meters_per_second_t)>
+      m_outputVel;
+
+  Timer m_timer;
+  units::second_t m_prevTime;
+  frc::DifferentialDriveWheelSpeeds m_prevSpeeds;
+  bool m_usePID;
+};
+}  // namespace frc2

wpilibNewCommands/src/main/native/include/frc2/command/RunCommand.h

@@ -0,0 +1,41 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "CommandBase.h"
+#include "CommandHelper.h"
+
+namespace frc2 {
+/**
+ * A command that runs a Runnable continuously.  Has no end condition as-is;
+ * either subclass it or use Command.WithTimeout() or
+ * Command.WithInterrupt() to give it one.  If you only wish
+ * to execute a Runnable once, use InstantCommand.
+ */
+class RunCommand : public CommandHelper<CommandBase, RunCommand> {
+ public:
+  /**
+   * 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
+   */
+  RunCommand(std::function<void()> toRun,
+             std::initializer_list<Subsystem*> requirements);
+
+  RunCommand(RunCommand&& other) = default;
+
+  RunCommand(const RunCommand& other) = default;
+
+  void Execute();
+
+ protected:
+  std::function<void()> m_toRun;
+};
+}  // namespace frc2

wpilibNewCommands/src/main/native/include/frc2/command/ScheduleCommand.h

@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <wpi/SmallVector.h>
+
+#include "CommandBase.h"
+#include "CommandHelper.h"
+#include "SetUtilities.h"
+
+namespace frc2 {
+/**
+ * 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.
+ */
+class ScheduleCommand : public CommandHelper<CommandBase, ScheduleCommand> {
+ public:
+  /**
+   * Creates a new ScheduleCommand that schedules the given commands when
+   * initialized.
+   *
+   * @param toSchedule the commands to schedule
+   */
+  explicit ScheduleCommand(wpi::ArrayRef<Command*> toSchedule);
+
+  ScheduleCommand(ScheduleCommand&& other) = default;
+
+  ScheduleCommand(const ScheduleCommand& other) = default;
+
+  void Initialize() override;
+
+  bool IsFinished() override;
+
+  bool RunsWhenDisabled() const override;
+
+ private:
+  wpi::SmallVector<Command*, 4> m_toSchedule;
+};
+}  // namespace frc2

wpilibNewCommands/src/main/native/include/frc2/command/SelectCommand.h

@@ -0,0 +1,153 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#ifdef _WIN32
+#pragma warning(push)
+#pragma warning(disable : 4521)
+#endif
+
+#include <memory>
+#include <unordered_map>
+#include <utility>
+#include <vector>
+
+#include "CommandBase.h"
+#include "CommandGroupBase.h"
+#include "PrintCommand.h"
+
+namespace frc2 {
+template <typename Key>
+/**
+ * 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 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.
+ */
+class SelectCommand : public CommandHelper<CommandBase, SelectCommand<Key>> {
+ public:
+  /**
+   * Creates a new selectcommand.
+   *
+   * @param commands the map of commands to choose from
+   * @param selector the selector to determine which command to run
+   */
+  template <class... Types,
+            typename = std::enable_if_t<std::conjunction_v<
+                std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
+  SelectCommand(std::function<Key()> selector,
+                std::pair<Key, Types>... commands)
+      : m_selector{std::move(selector)} {
+    std::vector<std::pair<Key, std::unique_ptr<Command>>> foo;
+
+    ((void)foo.emplace_back(commands.first,
+                            std::make_unique<std::remove_reference_t<Types>>(
+                                std::move(commands.second))),
+     ...);
+
+    for (auto&& command : foo) {
+      if (!CommandGroupBase::RequireUngrouped(command.second)) {
+        return;
+      }
+    }
+
+    for (auto&& command : foo) {
+      this->AddRequirements(command.second->GetRequirements());
+      m_runsWhenDisabled &= command.second->RunsWhenDisabled();
+      m_commands.emplace(std::move(command.first), std::move(command.second));
+    }
+  }
+
+  SelectCommand(
+      std::function<Key()> selector,
+      std::vector<std::pair<Key, std::unique_ptr<Command>>>&& commands)
+      : m_selector{std::move(selector)} {
+    for (auto&& command : commands) {
+      if (!CommandGroupBase::RequireUngrouped(command.second)) {
+        return;
+      }
+    }
+
+    for (auto&& command : commands) {
+      this->AddRequirements(command.second->GetRequirements());
+      m_runsWhenDisabled &= command.second->RunsWhenDisabled();
+      m_commands.emplace(std::move(command.first), std::move(command.second));
+    }
+  }
+
+  // No copy constructors for command groups
+  SelectCommand(const SelectCommand& other) = delete;
+
+  // Prevent template expansion from emulating copy ctor
+  SelectCommand(SelectCommand&) = delete;
+
+  /**
+   * Creates a new selectcommand.
+   *
+   * @param toRun a supplier providing the command to run
+   */
+  explicit SelectCommand(std::function<Command*()> toRun) : m_toRun{toRun} {}
+
+  SelectCommand(SelectCommand&& other) = default;
+
+  void Initialize() override;
+
+  void Execute() override { m_selectedCommand->Execute(); }
+
+  void End(bool interrupted) override {
+    return m_selectedCommand->End(interrupted);
+  }
+
+  bool IsFinished() override { return m_selectedCommand->IsFinished(); }
+
+  bool RunsWhenDisabled() const override { return m_runsWhenDisabled; }
+
+ protected:
+  std::unique_ptr<Command> TransferOwnership() && override {
+    return std::make_unique<SelectCommand>(std::move(*this));
+  }
+
+ private:
+  std::unordered_map<Key, std::unique_ptr<Command>> m_commands;
+  std::function<Key()> m_selector;
+  std::function<Command*()> m_toRun;
+  Command* m_selectedCommand;
+  bool m_runsWhenDisabled = true;
+};
+
+template <typename T>
+void SelectCommand<T>::Initialize() {
+  if (m_selector) {
+    auto find = m_commands.find(m_selector());
+    if (find == m_commands.end()) {
+      m_selectedCommand = new PrintCommand(
+          "SelectCommand selector value does not correspond to any command!");
+      return;
+    }
+    m_selectedCommand = find->second.get();
+  } else {
+    m_selectedCommand = m_toRun();
+  }
+  m_selectedCommand->Initialize();
+}
+
+}  // namespace frc2
+
+#ifdef _WIN32
+#pragma warning(pop)
+#endif

wpilibNewCommands/src/main/native/include/frc2/command/SequentialCommandGroup.h

@@ -0,0 +1,104 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#ifdef _WIN32
+#pragma warning(push)
+#pragma warning(disable : 4521)
+#endif
+
+#include <limits>
+#include <memory>
+#include <utility>
+#include <vector>
+
+#include <wpi/ArrayRef.h>
+
+#include "CommandGroupBase.h"
+#include "CommandHelper.h"
+#include "frc/ErrorBase.h"
+#include "frc/WPIErrors.h"
+
+namespace frc2 {
+
+const size_t invalid_index = std::numeric_limits<size_t>::max();
+
+/**
+ * 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.
+ */
+class SequentialCommandGroup
+    : public CommandHelper<CommandGroupBase, SequentialCommandGroup> {
+ public:
+  /**
+   * 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.
+   */
+  explicit SequentialCommandGroup(
+      std::vector<std::unique_ptr<Command>>&& commands);
+
+  /**
+   * 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.
+   */
+  template <class... Types,
+            typename = std::enable_if_t<std::conjunction_v<
+                std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
+  explicit SequentialCommandGroup(Types&&... commands) {
+    AddCommands(std::forward<Types>(commands)...);
+  }
+
+  SequentialCommandGroup(SequentialCommandGroup&& other) = default;
+
+  // No copy constructors for command groups
+  SequentialCommandGroup(const SequentialCommandGroup&) = delete;
+
+  // Prevent template expansion from emulating copy ctor
+  SequentialCommandGroup(SequentialCommandGroup&) = delete;
+
+  template <class... Types,
+            typename = std::enable_if_t<std::conjunction_v<
+                std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
+  void AddCommands(Types&&... commands) {
+    std::vector<std::unique_ptr<Command>> foo;
+    ((void)foo.emplace_back(std::make_unique<std::remove_reference_t<Types>>(
+         std::forward<Types>(commands))),
+     ...);
+    AddCommands(std::move(foo));
+  }
+
+  void Initialize() override;
+
+  void Execute() override;
+
+  void End(bool interrupted) override;
+
+  bool IsFinished() override;
+
+  bool RunsWhenDisabled() const override;
+
+ private:
+  void AddCommands(std::vector<std::unique_ptr<Command>>&& commands) final;
+
+  wpi::SmallVector<std::unique_ptr<Command>, 4> m_commands;
+  size_t m_currentCommandIndex{invalid_index};
+  bool m_runWhenDisabled{true};
+};
+}  // namespace frc2
+
+#ifdef _WIN32
+#pragma warning(pop)
+#endif

wpilibNewCommands/src/main/native/include/frc2/command/SetUtilities.h

@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <wpi/ArrayRef.h>
+#include <wpi/SmallVector.h>
+
+namespace frc2 {
+template <typename T>
+void SetInsert(wpi::SmallVectorImpl<T*>& vector, wpi::ArrayRef<T*> toAdd) {
+  for (auto addCommand : toAdd) {
+    bool exists = false;
+    for (auto existingCommand : vector) {
+      if (addCommand == existingCommand) {
+        exists = true;
+        break;
+      }
+    }
+    if (!exists) {
+      vector.emplace_back(addCommand);
+    }
+  }
+}
+}  // namespace frc2

wpilibNewCommands/src/main/native/include/frc2/command/StartEndCommand.h

@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "CommandBase.h"
+#include "CommandHelper.h"
+
+namespace frc2 {
+/**
+ * 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 Command.WithTimeout() or Command.WithInterrupt() to give
+ * it one.
+ */
+class StartEndCommand : public CommandHelper<CommandBase, StartEndCommand> {
+ public:
+  /**
+   * 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
+   */
+  StartEndCommand(std::function<void()> onInit, std::function<void()> onEnd,
+                  std::initializer_list<Subsystem*> requirements);
+
+  StartEndCommand(StartEndCommand&& other) = default;
+
+  StartEndCommand(const StartEndCommand& other);
+
+  void Initialize() override;
+
+  void End(bool interrupted) override;
+
+ protected:
+  std::function<void()> m_onInit;
+  std::function<void()> m_onEnd;
+};
+}  // namespace frc2

wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h

@@ -0,0 +1,88 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc2/command/CommandScheduler.h>
+
+#include <utility>
+
+namespace frc2 {
+class 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 Commands.  Subsystems are used by the 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 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
+ * CommandScheduler.RegisterSubsystem() method in order for the
+ * Periodic() method to be called.  It is recommended that this method be called
+ * from the constructor of users' Subsystem implementations.  The
+ * SubsystemBase class offers a simple base for user implementations
+ * that handles this.
+ *
+ * @see Command
+ * @see CommandScheduler
+ * @see SubsystemBase
+ */
+class Subsystem {
+ public:
+  ~Subsystem();
+  /**
+   * This method is called periodically by the CommandScheduler.  Useful for
+   * updating subsystem-specific state that you don't want to offload to a
+   * 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.
+   */
+  virtual void Periodic();
+
+  /**
+   * Sets the default 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 IsFinished() method should always return false.  Will automatically
+   * register this subsystem with the CommandScheduler.
+   *
+   * @param defaultCommand the default command to associate with this subsystem
+   */
+  template <class T, typename = std::enable_if_t<std::is_base_of_v<
+                         Command, std::remove_reference_t<T>>>>
+  void SetDefaultCommand(T&& defaultCommand) {
+    CommandScheduler::GetInstance().SetDefaultCommand(
+        this, std::forward<T>(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
+   */
+  Command* GetDefaultCommand() const;
+
+  /**
+   * 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
+   */
+  Command* GetCurrentCommand() const;
+
+  /**
+   * Registers this subsystem with the CommandScheduler, allowing its
+   * Periodic() method to be called when the scheduler runs.
+   */
+  void Register();
+};
+}  // namespace frc2

wpilibNewCommands/src/main/native/include/frc2/command/SubsystemBase.h

@@ -0,0 +1,68 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/smartdashboard/Sendable.h>
+#include <frc/smartdashboard/SendableHelper.h>
+
+#include <string>
+
+#include "Subsystem.h"
+
+namespace frc2 {
+/**
+ * A base for subsystems that handles registration in the constructor, and
+ * provides a more intuitive method for setting the default command.
+ */
+class SubsystemBase : public Subsystem,
+                      public frc::Sendable,
+                      public frc::SendableHelper<SubsystemBase> {
+ public:
+  void InitSendable(frc::SendableBuilder& builder) override;
+
+  /**
+   * Gets the name of this Subsystem.
+   *
+   * @return Name
+   */
+  std::string GetName() const;
+
+  /**
+   * Sets the name of this Subsystem.
+   *
+   * @param name name
+   */
+  void SetName(const wpi::Twine& name);
+
+  /**
+   * Gets the subsystem name of this Subsystem.
+   *
+   * @return Subsystem name
+   */
+  std::string GetSubsystem() const;
+
+  /**
+   * Sets the subsystem name of this Subsystem.
+   *
+   * @param subsystem subsystem name
+   */
+  void SetSubsystem(const wpi::Twine& name);
+
+  /**
+   * Associate a Sendable with this Subsystem.
+   * Also update the child's name.
+   *
+   * @param name name to give child
+   * @param child sendable
+   */
+  void AddChild(std::string name, frc::Sendable* child);
+
+ protected:
+  SubsystemBase();
+};
+}  // namespace frc2

wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h

@@ -0,0 +1,55 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <frc/trajectory/TrapezoidProfile.h>
+#include <frc2/Timer.h>
+
+#include <functional>
+
+#include "CommandBase.h"
+#include "CommandHelper.h"
+
+#pragma once
+
+namespace frc2 {
+/**
+ * A command that runs a TrapezoidProfile.  Useful for smoothly controlling
+ * mechanism motion.
+ *
+ * @see TrapezoidProfile
+ */
+class TrapezoidProfileCommand
+    : public CommandHelper<CommandBase, TrapezoidProfileCommand> {
+ public:
+  /**
+   * Creates a new TrapezoidProfileCommand that will execute the given
+   * TrapezoidalProfile. Output will be piped to the provided consumer function.
+   *
+   * @param profile The motion profile to execute.
+   * @param output  The consumer for the profile output.
+   */
+  TrapezoidProfileCommand(
+      frc::TrapezoidProfile profile,
+      std::function<void(frc::TrapezoidProfile::State)> output,
+      std::initializer_list<Subsystem*> requirements);
+
+  void Initialize() override;
+
+  void Execute() override;
+
+  void End(bool interrupted) override;
+
+  bool IsFinished() override;
+
+ private:
+  frc::TrapezoidProfile m_profile;
+  std::function<void(frc::TrapezoidProfile::State)> m_output;
+
+  Timer m_timer;
+};
+
+}  // namespace frc2

wpilibNewCommands/src/main/native/include/frc2/command/WaitCommand.h

@@ -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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <units/units.h>
+#include <wpi/Twine.h>
+
+#include "CommandBase.h"
+#include "CommandHelper.h"
+#include "frc2/Timer.h"
+
+namespace frc2 {
+/**
+ * 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.
+ */
+class WaitCommand : public CommandHelper<CommandBase, WaitCommand> {
+ public:
+  /**
+   * Creates a new WaitCommand.  This command will do nothing, and end after the
+   * specified duration.
+   *
+   * @param duration the time to wait
+   */
+  explicit WaitCommand(units::second_t duration);
+
+  WaitCommand(WaitCommand&& other) = default;
+
+  WaitCommand(const WaitCommand& other) = default;
+
+  void Initialize() override;
+
+  void End(bool interrupted) override;
+
+  bool IsFinished() override;
+
+  bool RunsWhenDisabled() const override;
+
+ protected:
+  Timer m_timer;
+
+ private:
+  units::second_t m_duration;
+};
+}  // namespace frc2

wpilibNewCommands/src/main/native/include/frc2/command/WaitUntilCommand.h

@@ -0,0 +1,52 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "CommandBase.h"
+#include "frc/Timer.h"
+#include "frc2/command/CommandHelper.h"
+
+namespace frc2 {
+/**
+ * A command that does nothing but ends after a specified match time or
+ * condition.  Useful for CommandGroups.
+ */
+class WaitUntilCommand : public CommandHelper<CommandBase, WaitUntilCommand> {
+ public:
+  /**
+   * Creates a new WaitUntilCommand that ends after a given condition becomes
+   * true.
+   *
+   * @param condition the condition to determine when to end
+   */
+  explicit WaitUntilCommand(std::function<bool()> condition);
+
+  /**
+   * 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
+   */
+  explicit WaitUntilCommand(double time);
+
+  WaitUntilCommand(WaitUntilCommand&& other) = default;
+
+  WaitUntilCommand(const WaitUntilCommand& other) = default;
+
+  bool IsFinished() override;
+
+  bool RunsWhenDisabled() const override;
+
+ private:
+  std::function<bool()> m_condition;
+};
+}  // namespace frc2

wpilibNewCommands/src/main/native/include/frc2/command/button/Button.h

@@ -0,0 +1,207 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+#include <utility>
+
+#include "Trigger.h"
+
+namespace frc2 {
+class Command;
+/**
+ * A class used to bind command scheduling to button presses.  Can be composed
+ * with other buttons with the operators in Trigger.
+ *
+ * @see Trigger
+ */
+class Button : public Trigger {
+ public:
+  /**
+   * Create a new button that is pressed when the given condition is true.
+   *
+   * @param isActive Whether the button is pressed.
+   */
+  explicit Button(std::function<bool()> isPressed);
+
+  /**
+   * Create a new button that is pressed active (default constructor) - activity
+   *  can be further determined by subclass code.
+   */
+  Button() = default;
+
+  /**
+   * Binds a command to start when the button is pressed.  Takes a
+   * raw pointer, and so is non-owning; users are responsible for the lifespan
+   * of the command.
+   *
+   * @param command The command to bind.
+   * @param interruptible Whether the command should be interruptible.
+   * @return The trigger, for chained calls.
+   */
+  Button WhenPressed(Command* command, bool interruptible = true);
+
+  /**
+   * Binds a command to start when the button is pressed.  Transfers
+   * command ownership to the button scheduler, so the user does not have to
+   * worry about lifespan - rvalue refs will be *moved*, lvalue refs will be
+   * *copied.*
+   *
+   * @param command The command to bind.
+   * @param interruptible Whether the command should be interruptible.
+   * @return The trigger, for chained calls.
+   */
+  template <class T, typename = std::enable_if_t<std::is_base_of_v<
+                         Command, std::remove_reference_t<T>>>>
+  Button WhenPressed(T&& command, bool interruptible = true) {
+    WhenActive(std::forward<T>(command), interruptible);
+    return *this;
+  }
+
+  /**
+   * Binds a runnable to execute when the button is pressed.
+   *
+   * @param toRun the runnable to execute.
+   */
+  Button WhenPressed(std::function<void()> toRun);
+
+  /**
+   * Binds a command to be started repeatedly while the button is pressed, and
+   * cancelled when it is released.  Takes a raw pointer, and so is non-owning;
+   * users are responsible for the lifespan of the command.
+   *
+   * @param command The command to bind.
+   * @param interruptible Whether the command should be interruptible.
+   * @return The button, for chained calls.
+   */
+  Button WhileHeld(Command* command, bool interruptible = true);
+
+  /**
+   * Binds a command to be started repeatedly while the button is pressed, and
+   * cancelled when it is released.  Transfers command ownership to the button
+   * scheduler, so the user does not have to worry about lifespan - rvalue refs
+   * will be *moved*, lvalue refs will be *copied.*
+   *
+   * @param command The command to bind.
+   * @param interruptible Whether the command should be interruptible.
+   * @return The button, for chained calls.
+   */
+  template <class T, typename = std::enable_if_t<std::is_base_of_v<
+                         Command, std::remove_reference_t<T>>>>
+  Button WhileHeld(T&& command, bool interruptible = true) {
+    WhileActiveContinous(std::forward<T>(command), interruptible);
+    return *this;
+  }
+
+  /**
+   * Binds a runnable to execute repeatedly while the button is pressed.
+   *
+   * @param toRun the runnable to execute.
+   */
+  Button WhileHeld(std::function<void()> toRun);
+
+  /**
+   * Binds a command to be started when the button is pressed, and cancelled
+   * when it is released.  Takes a raw pointer, and so is non-owning; users are
+   * responsible for the lifespan of the command.
+   *
+   * @param command The command to bind.
+   * @param interruptible Whether the command should be interruptible.
+   * @return The button, for chained calls.
+   */
+  Button WhenHeld(Command* command, bool interruptible = true);
+
+  /**
+   * Binds a command to be started when the button is pressed, and cancelled
+   * when it is released.  Transfers command ownership to the button scheduler,
+   * so the user does not have to worry about lifespan - rvalue refs will be
+   * *moved*, lvalue refs will be *copied.*
+   *
+   * @param command The command to bind.
+   * @param interruptible Whether the command should be interruptible.
+   * @return The button, for chained calls.
+   */
+  template <class T, typename = std::enable_if_t<std::is_base_of_v<
+                         Command, std::remove_reference_t<T>>>>
+  Button WhenHeld(T&& command, bool interruptible = true) {
+    WhileActiveOnce(std::forward<T>(command), interruptible);
+    return *this;
+  }
+
+  /**
+   * Binds a command to start when the button is released.  Takes a
+   * raw pointer, and so is non-owning; users are responsible for the lifespan
+   * of the command.
+   *
+   * @param command The command to bind.
+   * @param interruptible Whether the command should be interruptible.
+   * @return The button, for chained calls.
+   */
+  Button WhenReleased(Command* command, bool interruptible = true);
+
+  /**
+   * Binds a command to start when the button is pressed.  Transfers
+   * command ownership to the button scheduler, so the user does not have to
+   * worry about lifespan - rvalue refs will be *moved*, lvalue refs will be
+   * *copied.*
+   *
+   * @param command The command to bind.
+   * @param interruptible Whether the command should be interruptible.
+   * @return The button, for chained calls.
+   */
+  template <class T, typename = std::enable_if_t<std::is_base_of_v<
+                         Command, std::remove_reference_t<T>>>>
+  Button WhenReleased(T&& command, bool interruptible = true) {
+    WhenInactive(std::forward<T>(command), interruptible);
+    return *this;
+  }
+
+  /**
+   * Binds a runnable to execute when the button is released.
+   *
+   * @param toRun the runnable to execute.
+   */
+  Button WhenReleased(std::function<void()> toRun);
+
+  /**
+   * Binds a command to start when the button is pressed, and be cancelled when
+   * it is pressed again.  Takes a raw pointer, and so is non-owning; users are
+   * responsible for the lifespan of the command.
+   *
+   * @param command The command to bind.
+   * @param interruptible Whether the command should be interruptible.
+   * @return The button, for chained calls.
+   */
+  Button ToggleWhenPressed(Command* command, bool interruptible = true);
+
+  /**
+   * Binds a command to start when the button is pressed, and be cancelled when
+   * it is pessed again.  Transfers command ownership to the button scheduler,
+   * so the user does not have to worry about lifespan - rvalue refs will be
+   * *moved*, lvalue refs will be *copied.*
+   *
+   * @param command The command to bind.
+   * @param interruptible Whether the command should be interruptible.
+   * @return The button, for chained calls.
+   */
+  template <class T, typename = std::enable_if_t<std::is_base_of_v<
+                         Command, std::remove_reference_t<T>>>>
+  Button ToggleWhenPressed(T&& command, bool interruptible = true) {
+    ToggleWhenActive(std::forward<T>(command), interruptible);
+    return *this;
+  }
+
+  /**
+   * Binds a command to be cancelled when the button is pressed.  Takes a
+   * raw pointer, and so is non-owning; users are responsible for the lifespan
+   *  and scheduling of the command.
+   *
+   * @param command The command to bind.
+   * @return The button, for chained calls.
+   */
+  Button CancelWhenPressed(Command* command);
+};
+}  // namespace frc2

wpilibNewCommands/src/main/native/include/frc2/command/button/JoystickButton.h

@@ -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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+#include <frc/GenericHID.h>
+
+#include "Button.h"
+
+namespace frc2 {
+/**
+ * A class used to bind command scheduling to joystick button presses.  Can be
+ * composed with other buttons with the operators in Trigger.
+ *
+ * @see Trigger
+ */
+class JoystickButton : public Button {
+ public:
+  /**
+   * Creates a JoystickButton that commands can be bound to.
+   *
+   * @param joystick The joystick on which the button is located.
+   * @param buttonNumber The number of the button on the joystic.
+   */
+  explicit JoystickButton(frc::GenericHID* joystick, int buttonNumber)
+      : m_joystick{joystick}, m_buttonNumber{buttonNumber} {}
+
+  bool Get() const override { return m_joystick->GetRawButton(m_buttonNumber); }
+
+ private:
+  frc::GenericHID* m_joystick;
+  int m_buttonNumber;
+};
+}  // namespace frc2