diff --git a/wpilibNewCommands/src/dev/java/edu/wpi/first/wpilibj2/commands/DevMain.java b/wpilibNewCommands/src/dev/java/edu/wpi/first/wpilibj2/commands/DevMain.java
new file mode 100644
index 0000000..ca6ce63
--- /dev/null
+++ b/wpilibNewCommands/src/dev/java/edu/wpi/first/wpilibj2/commands/DevMain.java
@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.commands;
+
+import edu.wpi.first.hal.HALUtil;
+import edu.wpi.first.networktables.NetworkTablesJNI;
+import edu.wpi.first.wpiutil.RuntimeDetector;
+
+public final class DevMain {
+  /**
+   * Main entry point.
+   */
+  public static void main(String[] args) {
+    System.out.println("Hello World!");
+    System.out.println(RuntimeDetector.getPlatformPath());
+    System.out.println(NetworkTablesJNI.now());
+    System.out.println(HALUtil.getHALRuntimeType());
+  }
+
+  private DevMain() {
+  }
+}
diff --git a/wpilibNewCommands/src/dev/native/cpp/main.cpp b/wpilibNewCommands/src/dev/native/cpp/main.cpp
new file mode 100644
index 0000000..5312a1d
--- /dev/null
+++ b/wpilibNewCommands/src/dev/native/cpp/main.cpp
@@ -0,0 +1,8 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+int main() {}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java
new file mode 100644
index 0000000..6cca974
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java
@@ -0,0 +1,311 @@
+/*----------------------------------------------------------------------------*/
+/* 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
+   * @param requirements the required subsystems
+   * @return the decorated command
+   */
+  default SequentialCommandGroup beforeStarting(Runnable toRun, Subsystem... requirements) {
+    return new SequentialCommandGroup(new InstantCommand(toRun, requirements), 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
+   * @param requirements the required subsystems
+   * @return the decorated command
+   */
+  default SequentialCommandGroup andThen(Runnable toRun, Subsystem... requirements) {
+    return new SequentialCommandGroup(this, new InstantCommand(toRun, requirements));
+  }
+
+  /**
+   * 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();
+  }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandBase.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandBase.java
new file mode 100644
index 0000000..ba2c68f
--- /dev/null
+++ b/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);
+  }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandGroupBase.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandGroupBase.java
new file mode 100644
index 0000000..6ac0667
--- /dev/null
+++ b/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);
+  }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java
new file mode 100644
index 0000000..d82e77f
--- /dev/null
+++ b/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.kCommand2_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]));
+    });
+  }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandState.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandState.java
new file mode 100644
index 0000000..2e2dc7e
--- /dev/null
+++ b/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;
+  }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ConditionalCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ConditionalCommand.java
new file mode 100644
index 0000000..8e3239c
--- /dev/null
+++ b/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();
+  }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/FunctionalCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/FunctionalCommand.java
new file mode 100644
index 0000000..b9dd6db
--- /dev/null
+++ b/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();
+  }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/InstantCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/InstantCommand.java
new file mode 100644
index 0000000..8518890
--- /dev/null
+++ b/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;
+  }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java
new file mode 100644
index 0000000..af50dc8
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java
@@ -0,0 +1,353 @@
+/*----------------------------------------------------------------------------*/
+/* 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 java.util.function.Supplier;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
+import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.kinematics.MecanumDriveKinematics;
+import edu.wpi.first.wpilibj.kinematics.MecanumDriveMotorVoltages;
+import edu.wpi.first.wpilibj.kinematics.MecanumDriveWheelSpeeds;
+import edu.wpi.first.wpilibj.trajectory.Trajectory;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * A command that uses two PID controllers ({@link PIDController}) and a
+ * ProfiledPIDController ({@link ProfiledPIDController}) to follow a trajectory
+ * {@link Trajectory} with a mecanum drive.
+ *
+ * <p>The command handles trajectory-following,
+ * Velocity 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 PID
+ * controllers.
+ *
+ * <p>The robot angle controller does not follow the angle given by
+ * the trajectory but rather goes to the angle given in the final state of the trajectory.
+ */
+
+@SuppressWarnings({"PMD.TooManyFields", "MemberName"})
+public class MecanumControllerCommand extends CommandBase {
+  private final Timer m_timer = new Timer();
+  private MecanumDriveWheelSpeeds m_prevSpeeds;
+  private double m_prevTime;
+  private Pose2d m_finalPose;
+  private final boolean m_usePID;
+
+  private final Trajectory m_trajectory;
+  private final Supplier<Pose2d> m_pose;
+  private final SimpleMotorFeedforward m_feedforward;
+  private final MecanumDriveKinematics m_kinematics;
+  private final PIDController m_xController;
+  private final PIDController m_yController;
+  private final ProfiledPIDController m_thetaController;
+  private final double m_maxWheelVelocityMetersPerSecond;
+  private final PIDController m_frontLeftController;
+  private final PIDController m_rearLeftController;
+  private final PIDController m_frontRightController;
+  private final PIDController m_rearRightController;
+  private final Supplier<MecanumDriveWheelSpeeds> m_currentWheelSpeeds;
+  private final Consumer<MecanumDriveMotorVoltages> m_outputDriveVoltages;
+  private final Consumer<MecanumDriveWheelSpeeds> m_outputWheelSpeeds;
+
+  /**
+   * Constructs a new MecanumControllerCommand that when executed will follow the provided
+   * trajectory. PID control and feedforward are handled internally. Outputs are scaled from -12 to
+   * 12 as a voltage output to the motor.
+   *
+   * <p>Note: The controllers 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.
+   *
+   * <p>Note 2: The rotation controller will calculate the rotation based on the final pose in the
+   * trajectory, not the poses at each time step.
+   *
+   * @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 feedforward                     The feedforward to use for the drivetrain.
+   * @param kinematics                      The kinematics for the robot drivetrain.
+   * @param xController                     The Trajectory Tracker PID controller
+   *                                        for the robot's x position.
+   * @param yController                     The Trajectory Tracker PID controller
+   *                                        for the robot's y position.
+   * @param thetaController                 The Trajectory Tracker PID controller
+   *                                        for angle for the robot.
+   * @param maxWheelVelocityMetersPerSecond The maximum velocity of a drivetrain wheel.
+   * @param frontLeftController             The front left wheel velocity PID.
+   * @param rearLeftController              The rear left wheel velocity PID.
+   * @param frontRightController            The front right wheel velocity PID.
+   * @param rearRightController             The rear right wheel velocity PID.
+   * @param currentWheelSpeeds              A MecanumDriveWheelSpeeds object containing
+   *                                        the current wheel speeds.
+   * @param outputDriveVoltages             A MecanumDriveMotorVoltages object containing
+   *                                        the output motor voltages.
+   * @param requirements                    The subsystems to require.
+   */
+
+  @SuppressWarnings({"PMD.ExcessiveParameterList", "ParameterName"})
+  public MecanumControllerCommand(Trajectory trajectory,
+                                Supplier<Pose2d> pose,
+                                SimpleMotorFeedforward feedforward,
+                                MecanumDriveKinematics kinematics,
+
+                                PIDController xController,
+                                PIDController yController,
+                                ProfiledPIDController thetaController,
+
+                                double maxWheelVelocityMetersPerSecond,
+
+                                PIDController frontLeftController,
+                                PIDController rearLeftController,
+                                PIDController frontRightController,
+                                PIDController rearRightController,
+
+                                Supplier<MecanumDriveWheelSpeeds> currentWheelSpeeds,
+
+                                Consumer<MecanumDriveMotorVoltages> outputDriveVoltages,
+                                Subsystem... requirements) {
+    m_trajectory = requireNonNullParam(trajectory, "trajectory", "MecanumControllerCommand");
+    m_pose = requireNonNullParam(pose, "pose", "MecanumControllerCommand");
+    m_feedforward = requireNonNullParam(feedforward, "feedforward", "MecanumControllerCommand");
+    m_kinematics = requireNonNullParam(kinematics, "kinematics", "MecanumControllerCommand");
+
+    m_xController = requireNonNullParam(xController, "xController",
+      "MecanumControllerCommand");
+    m_yController = requireNonNullParam(yController, "yController",
+      "MecanumControllerCommand");
+    m_thetaController = requireNonNullParam(thetaController, "thetaController",
+      "MecanumControllerCommand");
+
+    m_maxWheelVelocityMetersPerSecond = requireNonNullParam(maxWheelVelocityMetersPerSecond,
+      "maxWheelVelocityMetersPerSecond", "MecanumControllerCommand");
+
+    m_frontLeftController = requireNonNullParam(frontLeftController,
+      "frontLeftController", "MecanumControllerCommand");
+    m_rearLeftController = requireNonNullParam(rearLeftController,
+      "rearLeftController", "MecanumControllerCommand");
+    m_frontRightController = requireNonNullParam(frontRightController,
+      "frontRightController", "MecanumControllerCommand");
+    m_rearRightController = requireNonNullParam(rearRightController,
+      "rearRightController", "MecanumControllerCommand");
+
+    m_currentWheelSpeeds = requireNonNullParam(currentWheelSpeeds,
+      "currentWheelSpeeds", "MecanumControllerCommand");
+
+    m_outputDriveVoltages = requireNonNullParam(outputDriveVoltages,
+    "outputDriveVoltages", "MecanumControllerCommand");
+
+    m_outputWheelSpeeds = null;
+
+    m_usePID = true;
+
+    addRequirements(requirements);
+  }
+
+  /**
+   * Constructs a new MecanumControllerCommand that when executed will follow the provided
+   * trajectory. The user should implement a velocity PID on the desired output wheel velocities.
+   *
+   * <p>Note: The controllers 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 non-stationary end-states.
+   *
+   * <p>Note2: The rotation controller will calculate the rotation based on the final pose
+   * in the trajectory, not the poses at each time step.
+   *
+   * @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 kinematics                      The kinematics for the robot drivetrain.
+   * @param xController                     The Trajectory Tracker PID controller
+   *                                        for the robot's x position.
+   * @param yController                     The Trajectory Tracker PID controller
+   *                                        for the robot's y position.
+   * @param thetaController                 The Trajectory Tracker PID controller
+   *                                        for angle for the robot.
+   * @param maxWheelVelocityMetersPerSecond The maximum velocity of a drivetrain wheel.
+   * @param outputWheelSpeeds               A MecanumDriveWheelSpeeds object containing
+   *                                        the output wheel speeds.
+   * @param requirements                    The subsystems to require.
+   */
+
+  @SuppressWarnings({"PMD.ExcessiveParameterList", "ParameterName"})
+  public MecanumControllerCommand(Trajectory trajectory,
+                                Supplier<Pose2d> pose,
+                                MecanumDriveKinematics kinematics,
+                                PIDController xController,
+                                PIDController yController,
+                                ProfiledPIDController thetaController,
+
+                                double maxWheelVelocityMetersPerSecond,
+
+                                Consumer<MecanumDriveWheelSpeeds> outputWheelSpeeds,
+                                Subsystem... requirements) {
+    m_trajectory = requireNonNullParam(trajectory, "trajectory", "MecanumControllerCommand");
+    m_pose = requireNonNullParam(pose, "pose", "MecanumControllerCommand");
+    m_feedforward = new SimpleMotorFeedforward(0, 0, 0);
+    m_kinematics = requireNonNullParam(kinematics,
+      "kinematics", "MecanumControllerCommand");
+
+    m_xController = requireNonNullParam(xController,
+      "xController", "MecanumControllerCommand");
+    m_yController = requireNonNullParam(yController,
+      "xController", "MecanumControllerCommand");
+    m_thetaController = requireNonNullParam(thetaController,
+      "thetaController", "MecanumControllerCommand");
+
+    m_maxWheelVelocityMetersPerSecond = requireNonNullParam(maxWheelVelocityMetersPerSecond,
+      "maxWheelVelocityMetersPerSecond", "MecanumControllerCommand");
+
+    m_frontLeftController = null;
+    m_rearLeftController = null;
+    m_frontRightController = null;
+    m_rearRightController = null;
+
+    m_currentWheelSpeeds = null;
+
+    m_outputWheelSpeeds = requireNonNullParam(outputWheelSpeeds,
+      "outputWheelSpeeds", "MecanumControllerCommand");
+
+    m_outputDriveVoltages = null;
+
+    m_usePID = false;
+
+    addRequirements(requirements);
+  }
+
+  @Override
+  public void initialize() {
+    var initialState = m_trajectory.sample(0);
+
+    // Sample final pose to get robot rotation
+    m_finalPose = m_trajectory.sample(m_trajectory.getTotalTimeSeconds()).poseMeters;
+
+    var initialXVelocity = initialState.velocityMetersPerSecond
+        * initialState.poseMeters.getRotation().getCos();
+    var initialYVelocity = initialState.velocityMetersPerSecond
+        * initialState.poseMeters.getRotation().getSin();
+
+    m_prevSpeeds = m_kinematics.toWheelSpeeds(
+      new ChassisSpeeds(initialXVelocity, initialYVelocity, 0.0));
+
+    m_timer.reset();
+    m_timer.start();
+  }
+
+  @Override
+  @SuppressWarnings("LocalVariableName")
+  public void execute() {
+    double curTime = m_timer.get();
+    double dt = curTime - m_prevTime;
+
+    var desiredState = m_trajectory.sample(curTime);
+    var desiredPose = desiredState.poseMeters;
+
+    var poseError = desiredPose.relativeTo(m_pose.get());
+
+    double targetXVel = m_xController.calculate(
+        m_pose.get().getTranslation().getX(),
+        desiredPose.getTranslation().getX());
+
+    double targetYVel = m_yController.calculate(
+        m_pose.get().getTranslation().getY(),
+        desiredPose.getTranslation().getY());
+
+    // The robot will go to the desired rotation of the final pose in the trajectory,
+    // not following the poses at individual states.
+    double targetAngularVel = m_thetaController.calculate(
+        m_pose.get().getRotation().getRadians(),
+        m_finalPose.getRotation().getRadians());
+
+    double vRef = desiredState.velocityMetersPerSecond;
+
+    targetXVel += vRef * poseError.getRotation().getCos();
+    targetYVel += vRef * poseError.getRotation().getSin();
+
+    var targetChassisSpeeds = new ChassisSpeeds(targetXVel, targetYVel, targetAngularVel);
+
+    var targetWheelSpeeds = m_kinematics.toWheelSpeeds(targetChassisSpeeds);
+
+    targetWheelSpeeds.normalize(m_maxWheelVelocityMetersPerSecond);
+
+    var frontLeftSpeedSetpoint = targetWheelSpeeds.frontLeftMetersPerSecond;
+    var rearLeftSpeedSetpoint = targetWheelSpeeds.rearLeftMetersPerSecond;
+    var frontRightSpeedSetpoint = targetWheelSpeeds.frontRightMetersPerSecond;
+    var rearRightSpeedSetpoint =  targetWheelSpeeds.rearRightMetersPerSecond;
+
+    double frontLeftOutput;
+    double rearLeftOutput;
+    double frontRightOutput;
+    double rearRightOutput;
+
+    if (m_usePID) {
+      final double frontLeftFeedforward = m_feedforward.calculate(frontLeftSpeedSetpoint,
+              (frontLeftSpeedSetpoint - m_prevSpeeds.frontLeftMetersPerSecond) / dt);
+
+      final double rearLeftFeedforward = m_feedforward.calculate(rearLeftSpeedSetpoint,
+              (rearLeftSpeedSetpoint - m_prevSpeeds.rearLeftMetersPerSecond) / dt);
+
+      final double frontRightFeedforward = m_feedforward.calculate(frontRightSpeedSetpoint,
+              (frontRightSpeedSetpoint - m_prevSpeeds.frontRightMetersPerSecond) / dt);
+
+      final double rearRightFeedforward = m_feedforward.calculate(rearRightSpeedSetpoint,
+            (rearRightSpeedSetpoint - m_prevSpeeds.rearRightMetersPerSecond) / dt);
+
+      frontLeftOutput = frontLeftFeedforward + m_frontLeftController.calculate(
+            m_currentWheelSpeeds.get().frontLeftMetersPerSecond,
+             frontLeftSpeedSetpoint);
+
+      rearLeftOutput = rearLeftFeedforward + m_rearLeftController.calculate(
+            m_currentWheelSpeeds.get().rearLeftMetersPerSecond,
+             rearLeftSpeedSetpoint);
+
+      frontRightOutput = frontRightFeedforward + m_frontRightController.calculate(
+            m_currentWheelSpeeds.get().frontRightMetersPerSecond,
+             frontRightSpeedSetpoint);
+
+      rearRightOutput = rearRightFeedforward + m_rearRightController.calculate(
+            m_currentWheelSpeeds.get().rearRightMetersPerSecond,
+             rearRightSpeedSetpoint);
+
+      m_outputDriveVoltages.accept(new MecanumDriveMotorVoltages(
+          frontLeftOutput,
+          frontRightOutput,
+          rearLeftOutput,
+          rearRightOutput));
+
+    } else {
+      m_outputWheelSpeeds.accept(new MecanumDriveWheelSpeeds(
+          frontLeftSpeedSetpoint,
+          frontRightSpeedSetpoint,
+          rearLeftSpeedSetpoint,
+          rearRightSpeedSetpoint));
+    }
+
+    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());
+  }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/NotifierCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/NotifierCommand.java
new file mode 100644
index 0000000..e63fcc5
--- /dev/null
+++ b/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();
+  }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDCommand.java
new file mode 100644
index 0000000..5f2b093
--- /dev/null
+++ b/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;
+  }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDSubsystem.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDSubsystem.java
new file mode 100644
index 0000000..f9337c6
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDSubsystem.java
@@ -0,0 +1,103 @@
+/*----------------------------------------------------------------------------*/
+/* 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;
+
+  private double m_setpoint;
+
+  /**
+   * Creates a new PIDSubsystem.
+   *
+   * @param controller the PIDController to use
+   * @param initialPosition the initial setpoint of the subsystem
+   */
+  public PIDSubsystem(PIDController controller, double initialPosition) {
+    setSetpoint(initialPosition);
+    m_controller = requireNonNullParam(controller, "controller", "PIDSubsystem");
+  }
+
+  /**
+   * Creates a new PIDSubsystem.  Initial setpoint is zero.
+   *
+   * @param controller the PIDController to use
+   */
+  public PIDSubsystem(PIDController controller) {
+    this(controller, 0);
+  }
+
+  @Override
+  public void periodic() {
+    if (m_enabled) {
+      useOutput(m_controller.calculate(getMeasurement(), m_setpoint), m_setpoint);
+    }
+  }
+
+  public PIDController getController() {
+    return m_controller;
+  }
+
+  /**
+   * Sets the setpoint for the subsystem.
+   *
+   * @param setpoint the setpoint for the subsystem
+   */
+  public void setSetpoint(double setpoint) {
+    m_setpoint = setpoint;
+  }
+
+  /**
+   * Uses the output from the PIDController.
+   *
+   * @param output the output of the PIDController
+   * @param setpoint the setpoint of the PIDController (for feedforward)
+   */
+  protected abstract void useOutput(double output, double setpoint);
+
+  /**
+   * Returns the measurement of the process variable used by the PIDController.
+   *
+   * @return the measurement of the process variable
+   */
+  protected 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, 0);
+  }
+
+  /**
+   * Returns whether the controller is enabled.
+   *
+   * @return Whether the controller is enabled.
+   */
+  public boolean isEnabled() {
+    return m_enabled;
+  }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroup.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroup.java
new file mode 100644
index 0000000..38cc3c1
--- /dev/null
+++ b/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;
+  }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroup.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroup.java
new file mode 100644
index 0000000..37be702
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroup.java
@@ -0,0 +1,123 @@
+/*----------------------------------------------------------------------------*/
+/* 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 boolean m_finished = 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_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.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);
+    }
+    m_finished = false;
+  }
+
+  @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);
+        if (commandRunning.getKey() == m_deadline) {
+          m_finished = true;
+        }
+      }
+    }
+  }
+
+  @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_finished;
+  }
+
+  @Override
+  public boolean runsWhenDisabled() {
+    return m_runWhenDisabled;
+  }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroup.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroup.java
new file mode 100644
index 0000000..dcaf5f5
--- /dev/null
+++ b/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;
+  }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PerpetualCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PerpetualCommand.java
new file mode 100644
index 0000000..6ebb376
--- /dev/null
+++ b/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();
+  }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PrintCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PrintCommand.java
new file mode 100644
index 0000000..4fb4126
--- /dev/null
+++ b/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;
+  }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDCommand.java
new file mode 100644
index 0000000..96e18a0
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDCommand.java
@@ -0,0 +1,138 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 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(m_measurement.getAsDouble());
+  }
+
+  @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.0, new State());
+  }
+
+  /**
+   * Returns the ProfiledPIDController used by the command.
+   *
+   * @return The ProfiledPIDController
+   */
+  public ProfiledPIDController getController() {
+    return m_controller;
+  }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDSubsystem.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDSubsystem.java
new file mode 100644
index 0000000..2385ada
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDSubsystem.java
@@ -0,0 +1,115 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 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 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 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;
+
+  private TrapezoidProfile.State m_goal;
+
+  /**
+   * Creates a new ProfiledPIDSubsystem.
+   *
+   * @param controller the ProfiledPIDController to use
+   * @param initialPosition the initial goal position of the controller
+   */
+  public ProfiledPIDSubsystem(ProfiledPIDController controller,
+                              double initialPosition) {
+    m_controller = requireNonNullParam(controller, "controller", "ProfiledPIDSubsystem");
+    setGoal(initialPosition);
+  }
+
+  /**
+   * Creates a new ProfiledPIDSubsystem.  Initial goal position is zero.
+   *
+   * @param controller the ProfiledPIDController to use
+   */
+  public ProfiledPIDSubsystem(ProfiledPIDController controller) {
+    this(controller, 0);
+  }
+
+  @Override
+  public void periodic() {
+    if (m_enabled) {
+      useOutput(m_controller.calculate(getMeasurement(), m_goal), m_controller.getSetpoint());
+    }
+  }
+
+  public ProfiledPIDController getController() {
+    return m_controller;
+  }
+
+  /**
+   * Sets the goal state for the subsystem.
+   *
+   * @param goal The goal state for the subsystem's motion profile.
+   */
+  public void setGoal(TrapezoidProfile.State goal) {
+    m_goal = goal;
+  }
+
+  /**
+   * Sets the goal state for the subsystem.  Goal velocity assumed to be zero.
+   *
+   * @param goal The goal position for the subsystem's motion profile.
+   */
+  public void setGoal(double goal) {
+    setGoal(new TrapezoidProfile.State(goal, 0));
+  }
+
+  /**
+   * Uses the output from the ProfiledPIDController.
+   *
+   * @param output   the output of the ProfiledPIDController
+   * @param setpoint the setpoint state of the ProfiledPIDController, for feedforward
+   */
+  protected abstract void useOutput(double output, State setpoint);
+
+  /**
+   * Returns the measurement of the process variable used by the ProfiledPIDController.
+   *
+   * @return the measurement of the process variable
+   */
+  protected abstract double getMeasurement();
+
+  /**
+   * Enables the PID control.  Resets the controller.
+   */
+  public void enable() {
+    m_enabled = true;
+    m_controller.reset(getMeasurement());
+  }
+
+  /**
+   * Disables the PID control.  Sets output to zero.
+   */
+  public void disable() {
+    m_enabled = false;
+    useOutput(0, new State());
+  }
+
+  /**
+   * Returns whether the controller is enabled.
+   *
+   * @return Whether the controller is enabled.
+   */
+  public boolean isEnabled() {
+    return m_enabled;
+  }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProxyScheduleCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProxyScheduleCommand.java
new file mode 100644
index 0000000..27d67bc
--- /dev/null
+++ b/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;
+  }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java
new file mode 100644
index 0000000..eea06c7
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java
@@ -0,0 +1,206 @@
+/*----------------------------------------------------------------------------*/
+/* 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.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.controller.SimpleMotorFeedforward;
+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 final boolean m_usePID;
+  private final Trajectory m_trajectory;
+  private final Supplier<Pose2d> m_pose;
+  private final RamseteController m_follower;
+  private final SimpleMotorFeedforward m_feedforward;
+  private final DifferentialDriveKinematics m_kinematics;
+  private final Supplier<DifferentialDriveWheelSpeeds> m_speeds;
+  private final PIDController m_leftController;
+  private final PIDController m_rightController;
+  private final BiConsumer<Double, Double> m_output;
+  private DifferentialDriveWheelSpeeds m_prevSpeeds;
+  private double m_prevTime;
+
+  /**
+   * Constructs a new RamseteCommand that, when executed, will follow the provided trajectory.
+   * PID control and feedforward are handled internally, and outputs are scaled -12 to 12
+   * representing units of volts.
+   *
+   * <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 feedforward     The feedforward to use for the drive.
+   * @param kinematics      The kinematics for the robot drivetrain.
+   * @param wheelSpeeds     A function that supplies the speeds of the left and
+   *                        right sides 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,
+                        SimpleMotorFeedforward feedforward,
+                        DifferentialDriveKinematics kinematics,
+                        Supplier<DifferentialDriveWheelSpeeds> wheelSpeeds,
+                        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_feedforward = feedforward;
+    m_kinematics = requireNonNullParam(kinematics, "kinematics", "RamseteCommand");
+    m_speeds = requireNonNullParam(wheelSpeeds, "wheelSpeeds", "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_feedforward = null;
+    m_speeds = 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_feedforward.calculate(leftSpeedSetpoint,
+              (leftSpeedSetpoint - m_prevSpeeds.leftMetersPerSecond) / dt);
+
+      double rightFeedforward =
+          m_feedforward.calculate(rightSpeedSetpoint,
+              (rightSpeedSetpoint - m_prevSpeeds.rightMetersPerSecond) / dt);
+
+      leftOutput = leftFeedforward
+          + m_leftController.calculate(m_speeds.get().leftMetersPerSecond,
+          leftSpeedSetpoint);
+
+      rightOutput = rightFeedforward
+          + m_rightController.calculate(m_speeds.get().rightMetersPerSecond,
+          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());
+  }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RunCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RunCommand.java
new file mode 100644
index 0000000..9099be0
--- /dev/null
+++ b/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();
+  }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ScheduleCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ScheduleCommand.java
new file mode 100644
index 0000000..700925b
--- /dev/null
+++ b/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;
+  }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SelectCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SelectCommand.java
new file mode 100644
index 0000000..92fe07f
--- /dev/null
+++ b/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();
+    }
+  }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroup.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroup.java
new file mode 100644
index 0000000..0d01f11
--- /dev/null
+++ b/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;
+  }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/StartEndCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/StartEndCommand.java
new file mode 100644
index 0000000..3139465
--- /dev/null
+++ b/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();
+  }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Subsystem.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Subsystem.java
new file mode 100644
index 0000000..7dc917b
--- /dev/null
+++ b/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);
+  }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SubsystemBase.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SubsystemBase.java
new file mode 100644
index 0000000..e22a5c4
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SubsystemBase.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 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);
+  }
+
+  @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);
+  }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.java
new file mode 100644
index 0000000..b05dc20
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.java
@@ -0,0 +1,158 @@
+/*----------------------------------------------------------------------------*/
+/* 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 java.util.function.Supplier;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.wpilibj.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.trajectory.Trajectory;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * A command that uses two PID controllers ({@link PIDController}) and a
+ * ProfiledPIDController ({@link ProfiledPIDController}) to follow a trajectory
+ * {@link Trajectory} with a swerve drive.
+ *
+ * <p>This command outputs the raw desired Swerve Module States ({@link SwerveModuleState})
+ * in an array. The desired wheel and module rotation velocities should be taken
+ * from those and used in velocity PIDs.
+ *
+ * <p>The robot angle controller does not follow the angle given by
+ * the trajectory but rather goes to the angle given in the final state of the trajectory.
+ */
+
+@SuppressWarnings("MemberName")
+public class SwerveControllerCommand extends CommandBase {
+  private final Timer m_timer = new Timer();
+  private Pose2d m_finalPose;
+
+  private final Trajectory m_trajectory;
+  private final Supplier<Pose2d> m_pose;
+  private final SwerveDriveKinematics m_kinematics;
+  private final PIDController m_xController;
+  private final PIDController m_yController;
+  private final ProfiledPIDController m_thetaController;
+  private final Consumer<SwerveModuleState[]> m_outputModuleStates;
+
+  /**
+   * Constructs a new SwerveControllerCommand that when executed will follow the provided
+   * trajectory. This command will not return output voltages but rather raw module states from the
+   * position controllers which need to be put into a velocity PID.
+   *
+   * <p>Note: The controllers 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.
+   *
+   * <p>Note 2: The rotation controller will calculate the rotation based on the final pose
+   * in the trajectory, not the poses at each time step.
+   *
+   * @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 kinematics         The kinematics for the robot drivetrain.
+   * @param xController        The Trajectory Tracker PID controller
+   *                           for the robot's x position.
+   * @param yController        The Trajectory Tracker PID controller
+   *                           for the robot's y position.
+   * @param thetaController    The Trajectory Tracker PID controller
+   *                           for angle for the robot.
+   * @param outputModuleStates The raw output module states from the
+   *                           position controllers.
+   * @param requirements       The subsystems to require.
+   */
+
+  @SuppressWarnings("ParameterName")
+  public SwerveControllerCommand(Trajectory trajectory,
+                               Supplier<Pose2d> pose,
+                               SwerveDriveKinematics kinematics,
+                               PIDController xController,
+                               PIDController yController,
+                               ProfiledPIDController thetaController,
+
+                               Consumer<SwerveModuleState[]> outputModuleStates,
+                               Subsystem... requirements) {
+    m_trajectory = requireNonNullParam(trajectory, "trajectory", "SwerveControllerCommand");
+    m_pose = requireNonNullParam(pose, "pose", "SwerveControllerCommand");
+    m_kinematics = requireNonNullParam(kinematics, "kinematics", "SwerveControllerCommand");
+
+    m_xController = requireNonNullParam(xController,
+      "xController", "SwerveControllerCommand");
+    m_yController = requireNonNullParam(yController,
+      "xController", "SwerveControllerCommand");
+    m_thetaController = requireNonNullParam(thetaController,
+      "thetaController", "SwerveControllerCommand");
+
+    m_outputModuleStates = requireNonNullParam(outputModuleStates,
+      "frontLeftOutput", "SwerveControllerCommand");
+    addRequirements(requirements);
+  }
+
+  @Override
+  public void initialize() {
+    // Sample final pose to get robot rotation
+    m_finalPose = m_trajectory.sample(m_trajectory.getTotalTimeSeconds()).poseMeters;
+
+    m_timer.reset();
+    m_timer.start();
+  }
+
+  @Override
+  @SuppressWarnings("LocalVariableName")
+  public void execute() {
+    double curTime = m_timer.get();
+
+    var desiredState = m_trajectory.sample(curTime);
+    var desiredPose = desiredState.poseMeters;
+
+    var poseError = desiredPose.relativeTo(m_pose.get());
+
+    double targetXVel = m_xController.calculate(
+        m_pose.get().getTranslation().getX(),
+        desiredPose.getTranslation().getX());
+
+    double targetYVel = m_yController.calculate(
+        m_pose.get().getTranslation().getY(),
+        desiredPose.getTranslation().getY());
+
+    // The robot will go to the desired rotation of the final pose in the trajectory,
+    // not following the poses at individual states.
+    double targetAngularVel = m_thetaController.calculate(
+        m_pose.get().getRotation().getRadians(),
+        m_finalPose.getRotation().getRadians());
+
+    double vRef = desiredState.velocityMetersPerSecond;
+
+    targetXVel += vRef * poseError.getRotation().getCos();
+    targetYVel += vRef * poseError.getRotation().getSin();
+
+    var targetChassisSpeeds = new ChassisSpeeds(targetXVel, targetYVel, targetAngularVel);
+
+    var targetModuleStates = m_kinematics.toSwerveModuleStates(targetChassisSpeeds);
+
+    m_outputModuleStates.accept(targetModuleStates);
+
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    m_timer.stop();
+  }
+
+  @Override
+  public boolean isFinished() {
+    return m_timer.hasPeriodPassed(m_trajectory.getTotalTimeSeconds());
+  }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java
new file mode 100644
index 0000000..85ddd03
--- /dev/null
+++ b/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());
+  }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileSubsystem.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileSubsystem.java
new file mode 100644
index 0000000..05e6386
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileSubsystem.java
@@ -0,0 +1,112 @@
+/*----------------------------------------------------------------------------*/
+/* 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.trajectory.TrapezoidProfile;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * A subsystem that generates and runs trapezoidal motion profiles automatically.  The user
+ * specifies how to use the current state of the motion profile by overriding the `useState` method.
+ */
+public abstract class TrapezoidProfileSubsystem extends SubsystemBase {
+  private final double m_period;
+  private final TrapezoidProfile.Constraints m_constraints;
+
+  private TrapezoidProfile.State m_state;
+  private TrapezoidProfile.State m_goal;
+
+  private boolean m_enabled = true;
+
+  /**
+   * Creates a new TrapezoidProfileSubsystem.
+   *
+   * @param constraints     The constraints (maximum velocity and acceleration) for the profiles.
+   * @param initialPosition The initial position of the controlled mechanism when the subsystem
+   *                        is constructed.
+   * @param period          The period of the main robot loop, in seconds.
+   */
+  public TrapezoidProfileSubsystem(TrapezoidProfile.Constraints constraints,
+                                   double initialPosition,
+                                   double period) {
+    m_constraints = requireNonNullParam(constraints, "constraints", "TrapezoidProfileSubsystem");
+    m_state = new TrapezoidProfile.State(initialPosition, 0);
+    setGoal(initialPosition);
+    m_period = period;
+  }
+
+  /**
+   * Creates a new TrapezoidProfileSubsystem.
+   *
+   * @param constraints     The constraints (maximum velocity and acceleration) for the profiles.
+   * @param initialPosition The initial position of the controlled mechanism when the subsystem
+   *                        is constructed.
+   */
+  public TrapezoidProfileSubsystem(TrapezoidProfile.Constraints constraints,
+                                   double initialPosition) {
+    this(constraints, initialPosition, 0.02);
+  }
+
+  /**
+   * Creates a new TrapezoidProfileSubsystem.
+   *
+   * @param constraints     The constraints (maximum velocity and acceleration) for the profiles.
+   */
+  public TrapezoidProfileSubsystem(TrapezoidProfile.Constraints constraints) {
+    this(constraints, 0, 0.02);
+  }
+
+  @Override
+  public void periodic() {
+    var profile = new TrapezoidProfile(m_constraints, m_goal, m_state);
+    m_state = profile.calculate(m_period);
+    if (m_enabled) {
+      useState(m_state);
+    }
+  }
+
+  /**
+   * Sets the goal state for the subsystem.
+   *
+   * @param goal The goal state for the subsystem's motion profile.
+   */
+  public void setGoal(TrapezoidProfile.State goal) {
+    m_goal = goal;
+  }
+
+  /**
+   * Sets the goal state for the subsystem.  Goal velocity assumed to be zero.
+   *
+   * @param goal The goal position for the subsystem's motion profile.
+   */
+  public void setGoal(double goal) {
+    setGoal(new TrapezoidProfile.State(goal, 0));
+  }
+
+  /**
+   * Enable the TrapezoidProfileSubsystem's output.
+   */
+  public void enable() {
+    m_enabled = true;
+  }
+
+  /**
+   * Disable the TrapezoidProfileSubsystem's output.
+   */
+  public void disable() {
+    m_enabled = false;
+  }
+
+  /**
+   * Users should override this to consume the current state of the motion profile.
+   *
+   * @param state The current state of the motion profile.
+   */
+  protected abstract void useState(TrapezoidProfile.State state);
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WaitCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WaitCommand.java
new file mode 100644
index 0000000..5e8ebc3
--- /dev/null
+++ b/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;
+  }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WaitUntilCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WaitUntilCommand.java
new file mode 100644
index 0000000..5c55fff
--- /dev/null
+++ b/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;
+  }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Button.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Button.java
new file mode 100644
index 0000000..b7f26ae
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Button.java
@@ -0,0 +1,215 @@
+/*----------------------------------------------------------------------------*/
+/* 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.Subsystem;
+
+/**
+ * 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
+   * @param requirements the required subsystems
+   * @return this button, so calls can be chained
+   */
+  public Button whenPressed(final Runnable toRun, Subsystem... requirements) {
+    whenActive(toRun, requirements);
+    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
+   * @param requirements the required subsystems
+   * @return this button, so calls can be chained
+   */
+  public Button whileHeld(final Runnable toRun, Subsystem... requirements) {
+    whileActiveContinuous(toRun, requirements);
+    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
+   * @param requirements the required subsystems
+   * @return this button, so calls can be chained
+   */
+  public Button whenReleased(final Runnable toRun, Subsystem... requirements) {
+    whenInactive(toRun, requirements);
+    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;
+  }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/InternalButton.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/InternalButton.java
new file mode 100644
index 0000000..3f74f48
--- /dev/null
+++ b/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;
+  }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/JoystickButton.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/JoystickButton.java
new file mode 100644
index 0000000..918bb6a
--- /dev/null
+++ b/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);
+  }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/POVButton.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/POVButton.java
new file mode 100644
index 0000000..823b756
--- /dev/null
+++ b/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;
+  }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java
new file mode 100644
index 0000000..5167e4a
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java
@@ -0,0 +1,355 @@
+/*----------------------------------------------------------------------------*/
+/* 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 edu.wpi.first.wpilibj2.command.Subsystem;
+
+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
+   * @param requirements the required subsystems
+   * @return this trigger, so calls can be chained
+   */
+  public Trigger whenActive(final Runnable toRun, Subsystem... requirements) {
+    return whenActive(new InstantCommand(toRun, requirements));
+  }
+
+  /**
+   * 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
+   * @param requirements the required subsystems
+   * @return this trigger, so calls can be chained
+   */
+  public Trigger whileActiveContinuous(final Runnable toRun, Subsystem... requirements) {
+    return whileActiveContinuous(new InstantCommand(toRun, requirements));
+  }
+
+  /**
+   * 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
+   * @param requirements the required subsystems
+   * @return this trigger, so calls can be chained
+   */
+  public Trigger whenInactive(final Runnable toRun, Subsystem... requirements) {
+    return whenInactive(new InstantCommand(toRun, requirements));
+  }
+
+  /**
+   * 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());
+  }
+}
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp
new file mode 100644
index 0000000..248368d
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp
@@ -0,0 +1,124 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 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 "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::initializer_list<Subsystem*> requirements) && {
+  return std::move(*this).BeforeStarting(
+      std::move(toRun),
+      wpi::makeArrayRef(requirements.begin(), requirements.end()));
+}
+
+SequentialCommandGroup Command::BeforeStarting(
+    std::function<void()> toRun, wpi::ArrayRef<Subsystem*> requirements) && {
+  std::vector<std::unique_ptr<Command>> temp;
+  temp.emplace_back(
+      std::make_unique<InstantCommand>(std::move(toRun), requirements));
+  temp.emplace_back(std::move(*this).TransferOwnership());
+  return SequentialCommandGroup(std::move(temp));
+}
+
+SequentialCommandGroup Command::AndThen(
+    std::function<void()> toRun,
+    std::initializer_list<Subsystem*> requirements) && {
+  return std::move(*this).AndThen(
+      std::move(toRun),
+      wpi::makeArrayRef(requirements.begin(), requirements.end()));
+}
+
+SequentialCommandGroup Command::AndThen(
+    std::function<void()> toRun, wpi::ArrayRef<Subsystem*> requirements) && {
+  std::vector<std::unique_ptr<Command>> temp;
+  temp.emplace_back(std::move(*this).TransferOwnership());
+  temp.emplace_back(
+      std::make_unique<InstantCommand>(std::move(toRun), requirements));
+  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
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandBase.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandBase.cpp
new file mode 100644
index 0000000..f2cfa95
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandBase.cpp
@@ -0,0 +1,64 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 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>
+
+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::ArrayRef<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();
+                               }
+                             });
+}
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandGroupBase.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandGroupBase.cpp
new file mode 100644
index 0000000..8869d4c
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandGroupBase.cpp
@@ -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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/CommandGroupBase.h"
+
+#include <frc/WPIErrors.h>
+
+using namespace frc2;
+
+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;
+}
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp
new file mode 100644
index 0000000..842facb
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp
@@ -0,0 +1,415 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 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 <hal/FRCUsageReporting.h>
+#include <hal/HALBase.h>
+#include <networktables/NetworkTableEntry.h>
+#include <wpi/DenseMap.h>
+
+#include "frc2/command/CommandGroupBase.h"
+#include "frc2/command/CommandState.h"
+#include "frc2/command/Subsystem.h"
+
+using namespace frc2;
+
+class CommandScheduler::Impl {
+ public:
+  // A map from commands to their scheduling state.  Also used as a set of the
+  // currently-running commands.
+  wpi::DenseMap<Command*, CommandState> scheduledCommands;
+
+  // A map from required subsystems to their requiring commands.  Also used as a
+  // set of the currently-required subsystems.
+  wpi::DenseMap<Subsystem*, Command*> 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>> subsystems;
+
+  // The set of currently-registered buttons that will be polled every
+  // iteration.
+  wpi::SmallVector<wpi::unique_function<void()>, 4> buttons;
+
+  bool disabled{false};
+
+  // NetworkTable entries for use in Sendable impl
+  nt::NetworkTableEntry namesEntry;
+  nt::NetworkTableEntry idsEntry;
+  nt::NetworkTableEntry cancelEntry;
+
+  // Lists of user-supplied actions to be executed on scheduling events for
+  // every command.
+  wpi::SmallVector<Action, 4> initActions;
+  wpi::SmallVector<Action, 4> executeActions;
+  wpi::SmallVector<Action, 4> interruptActions;
+  wpi::SmallVector<Action, 4> finishActions;
+
+  // Flag and queues for avoiding concurrent modification if commands are
+  // scheduled/canceled during run
+
+  bool inRunLoop = false;
+  wpi::DenseMap<Command*, bool> toSchedule;
+  wpi::SmallVector<Command*, 4> toCancel;
+};
+
+template <typename TMap, typename TKey>
+static bool ContainsKey(const TMap& map, TKey keyToCheck) {
+  return map.find(keyToCheck) != map.end();
+}
+
+CommandScheduler::CommandScheduler() : m_impl(new Impl) {
+  HAL_Report(HALUsageReporting::kResourceType_Command,
+             HALUsageReporting::kCommand2_Scheduler);
+  frc::SendableRegistry::GetInstance().AddLW(this, "Scheduler");
+}
+
+CommandScheduler::~CommandScheduler() {}
+
+CommandScheduler& CommandScheduler::GetInstance() {
+  static CommandScheduler scheduler;
+  return scheduler;
+}
+
+void CommandScheduler::AddButton(wpi::unique_function<void()> button) {
+  m_impl->buttons.emplace_back(std::move(button));
+}
+
+void CommandScheduler::ClearButtons() { m_impl->buttons.clear(); }
+
+void CommandScheduler::Schedule(bool interruptible, Command* command) {
+  if (m_impl->inRunLoop) {
+    m_impl->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_impl->disabled ||
+      (frc::RobotState::IsDisabled() && !command->RunsWhenDisabled()) ||
+      ContainsKey(m_impl->scheduledCommands, command)) {
+    return;
+  }
+
+  const auto& requirements = command->GetRequirements();
+
+  wpi::SmallVector<Command*, 8> intersection;
+
+  bool isDisjoint = true;
+  bool allInterruptible = true;
+  for (auto&& i1 : m_impl->requirements) {
+    if (requirements.find(i1.first) != requirements.end()) {
+      isDisjoint = false;
+      allInterruptible &=
+          m_impl->scheduledCommands[i1.second].IsInterruptible();
+      intersection.emplace_back(i1.second);
+    }
+  }
+
+  if (isDisjoint || allInterruptible) {
+    if (allInterruptible) {
+      for (auto&& cmdToCancel : intersection) {
+        Cancel(cmdToCancel);
+      }
+    }
+    command->Initialize();
+    m_impl->scheduledCommands[command] = CommandState{interruptible};
+    for (auto&& action : m_impl->initActions) {
+      action(*command);
+    }
+    for (auto&& requirement : requirements) {
+      m_impl->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_impl->disabled) {
+    return;
+  }
+
+  // Run the periodic method of all registered subsystems.
+  for (auto&& subsystem : m_impl->subsystems) {
+    subsystem.getFirst()->Periodic();
+  }
+
+  // Poll buttons for new commands to add.
+  for (auto&& button : m_impl->buttons) {
+    button();
+  }
+
+  m_impl->inRunLoop = true;
+  // Run scheduled commands, remove finished commands.
+  for (auto iterator = m_impl->scheduledCommands.begin();
+       iterator != m_impl->scheduledCommands.end(); iterator++) {
+    Command* command = iterator->getFirst();
+
+    if (!command->RunsWhenDisabled() && frc::RobotState::IsDisabled()) {
+      Cancel(command);
+      continue;
+    }
+
+    command->Execute();
+    for (auto&& action : m_impl->executeActions) {
+      action(*command);
+    }
+
+    if (command->IsFinished()) {
+      command->End(false);
+      for (auto&& action : m_impl->finishActions) {
+        action(*command);
+      }
+
+      for (auto&& requirement : command->GetRequirements()) {
+        m_impl->requirements.erase(requirement);
+      }
+
+      m_impl->scheduledCommands.erase(iterator);
+    }
+  }
+  m_impl->inRunLoop = false;
+
+  for (auto&& commandInterruptible : m_impl->toSchedule) {
+    Schedule(commandInterruptible.second, commandInterruptible.first);
+  }
+
+  for (auto&& command : m_impl->toCancel) {
+    Cancel(command);
+  }
+
+  m_impl->toSchedule.clear();
+  m_impl->toCancel.clear();
+
+  // Add default commands for un-required registered subsystems.
+  for (auto&& subsystem : m_impl->subsystems) {
+    auto s = m_impl->requirements.find(subsystem.getFirst());
+    if (s == m_impl->requirements.end() && subsystem.getSecond()) {
+      Schedule({subsystem.getSecond().get()});
+    }
+  }
+}
+
+void CommandScheduler::RegisterSubsystem(Subsystem* subsystem) {
+  m_impl->subsystems[subsystem] = nullptr;
+}
+
+void CommandScheduler::UnregisterSubsystem(Subsystem* subsystem) {
+  auto s = m_impl->subsystems.find(subsystem);
+  if (s != m_impl->subsystems.end()) {
+    m_impl->subsystems.erase(s);
+  }
+}
+
+void CommandScheduler::RegisterSubsystem(
+    std::initializer_list<Subsystem*> subsystems) {
+  for (auto* subsystem : subsystems) {
+    RegisterSubsystem(subsystem);
+  }
+}
+
+void CommandScheduler::RegisterSubsystem(wpi::ArrayRef<Subsystem*> subsystems) {
+  for (auto* subsystem : subsystems) {
+    RegisterSubsystem(subsystem);
+  }
+}
+
+void CommandScheduler::UnregisterSubsystem(
+    std::initializer_list<Subsystem*> subsystems) {
+  for (auto* subsystem : subsystems) {
+    UnregisterSubsystem(subsystem);
+  }
+}
+
+void CommandScheduler::UnregisterSubsystem(
+    wpi::ArrayRef<Subsystem*> subsystems) {
+  for (auto* subsystem : subsystems) {
+    UnregisterSubsystem(subsystem);
+  }
+}
+
+Command* CommandScheduler::GetDefaultCommand(const Subsystem* subsystem) const {
+  auto&& find = m_impl->subsystems.find(subsystem);
+  if (find != m_impl->subsystems.end()) {
+    return find->second.get();
+  } else {
+    return nullptr;
+  }
+}
+
+void CommandScheduler::Cancel(Command* command) {
+  if (m_impl->inRunLoop) {
+    m_impl->toCancel.emplace_back(command);
+    return;
+  }
+
+  auto find = m_impl->scheduledCommands.find(command);
+  if (find == m_impl->scheduledCommands.end()) return;
+  command->End(true);
+  for (auto&& action : m_impl->interruptActions) {
+    action(*command);
+  }
+  m_impl->scheduledCommands.erase(find);
+  for (auto&& requirement : m_impl->requirements) {
+    if (requirement.second == command) {
+      m_impl->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_impl->scheduledCommands) {
+    Cancel(command.first);
+  }
+}
+
+double CommandScheduler::TimeSinceScheduled(const Command* command) const {
+  auto find = m_impl->scheduledCommands.find(command);
+  if (find != m_impl->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_impl->scheduledCommands.find(command) !=
+         m_impl->scheduledCommands.end();
+}
+
+Command* CommandScheduler::Requiring(const Subsystem* subsystem) const {
+  auto find = m_impl->requirements.find(subsystem);
+  if (find != m_impl->requirements.end()) {
+    return find->second;
+  } else {
+    return nullptr;
+  }
+}
+
+void CommandScheduler::Disable() { m_impl->disabled = true; }
+
+void CommandScheduler::Enable() { m_impl->disabled = false; }
+
+void CommandScheduler::OnCommandInitialize(Action action) {
+  m_impl->initActions.emplace_back(std::move(action));
+}
+
+void CommandScheduler::OnCommandExecute(Action action) {
+  m_impl->executeActions.emplace_back(std::move(action));
+}
+
+void CommandScheduler::OnCommandInterrupt(Action action) {
+  m_impl->interruptActions.emplace_back(std::move(action));
+}
+
+void CommandScheduler::OnCommandFinish(Action action) {
+  m_impl->finishActions.emplace_back(std::move(action));
+}
+
+void CommandScheduler::InitSendable(frc::SendableBuilder& builder) {
+  builder.SetSmartDashboardType("Scheduler");
+  m_impl->namesEntry = builder.GetEntry("Names");
+  m_impl->idsEntry = builder.GetEntry("Ids");
+  m_impl->cancelEntry = builder.GetEntry("Cancel");
+
+  builder.SetUpdateTable([this] {
+    double tmp[1];
+    tmp[0] = 0;
+    auto toCancel = m_impl->cancelEntry.GetDoubleArray(tmp);
+    for (auto cancel : toCancel) {
+      uintptr_t ptrTmp = static_cast<uintptr_t>(cancel);
+      Command* command = reinterpret_cast<Command*>(ptrTmp);
+      if (m_impl->scheduledCommands.find(command) !=
+          m_impl->scheduledCommands.end()) {
+        Cancel(command);
+      }
+      m_impl->cancelEntry.SetDoubleArray(wpi::ArrayRef<double>{});
+    }
+
+    wpi::SmallVector<std::string, 8> names;
+    wpi::SmallVector<double, 8> ids;
+    for (auto&& command : m_impl->scheduledCommands) {
+      names.emplace_back(command.first->GetName());
+      uintptr_t ptrTmp = reinterpret_cast<uintptr_t>(command.first);
+      ids.emplace_back(static_cast<double>(ptrTmp));
+    }
+    m_impl->namesEntry.SetStringArray(names);
+    m_impl->idsEntry.SetDoubleArray(ids);
+  });
+}
+
+void CommandScheduler::SetDefaultCommandImpl(Subsystem* subsystem,
+                                             std::unique_ptr<Command> command) {
+  m_impl->subsystems[subsystem] = std::move(command);
+}
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandState.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandState.cpp
new file mode 100644
index 0000000..b41fa70
--- /dev/null
+++ b/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;
+}
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/ConditionalCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/ConditionalCommand.cpp
new file mode 100644
index 0000000..2344513
--- /dev/null
+++ b/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; }
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/FunctionalCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/FunctionalCommand.cpp
new file mode 100644
index 0000000..ee28ba7
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/FunctionalCommand.cpp
@@ -0,0 +1,41 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 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,
+    std::initializer_list<Subsystem*> requirements)
+    : m_onInit{std::move(onInit)},
+      m_onExecute{std::move(onExecute)},
+      m_onEnd{std::move(onEnd)},
+      m_isFinished{std::move(isFinished)} {
+  AddRequirements(requirements);
+}
+
+FunctionalCommand::FunctionalCommand(std::function<void()> onInit,
+                                     std::function<void()> onExecute,
+                                     std::function<void(bool)> onEnd,
+                                     std::function<bool()> isFinished,
+                                     wpi::ArrayRef<Subsystem*> requirements)
+    : m_onInit{std::move(onInit)},
+      m_onExecute{std::move(onExecute)},
+      m_onEnd{std::move(onEnd)},
+      m_isFinished{std::move(isFinished)} {
+  AddRequirements(requirements);
+}
+
+void FunctionalCommand::Initialize() { m_onInit(); }
+
+void FunctionalCommand::Execute() { m_onExecute(); }
+
+void FunctionalCommand::End(bool interrupted) { m_onEnd(interrupted); }
+
+bool FunctionalCommand::IsFinished() { return m_isFinished(); }
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/InstantCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/InstantCommand.cpp
new file mode 100644
index 0000000..6f66c5c
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/InstantCommand.cpp
@@ -0,0 +1,28 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 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(std::function<void()> toRun,
+                               wpi::ArrayRef<Subsystem*> requirements)
+    : m_toRun{std::move(toRun)} {
+  AddRequirements(requirements);
+}
+
+InstantCommand::InstantCommand() : m_toRun{[] {}} {}
+
+void InstantCommand::Initialize() { m_toRun(); }
+
+bool InstantCommand::IsFinished() { return true; }
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp
new file mode 100644
index 0000000..8809803
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp
@@ -0,0 +1,251 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 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/MecanumControllerCommand.h"
+
+using namespace frc2;
+using namespace units;
+
+MecanumControllerCommand::MecanumControllerCommand(
+    frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+    frc::SimpleMotorFeedforward<units::meters> feedforward,
+    frc::MecanumDriveKinematics kinematics, frc2::PIDController xController,
+    frc2::PIDController yController,
+    frc::ProfiledPIDController<units::radians> thetaController,
+    units::meters_per_second_t maxWheelVelocity,
+    std::function<frc::MecanumDriveWheelSpeeds()> currentWheelSpeeds,
+    frc2::PIDController frontLeftController,
+    frc2::PIDController rearLeftController,
+    frc2::PIDController frontRightController,
+    frc2::PIDController rearRightController,
+    std::function<void(units::volt_t, units::volt_t, units::volt_t,
+                       units::volt_t)>
+        output,
+    std::initializer_list<Subsystem*> requirements)
+    : m_trajectory(trajectory),
+      m_pose(pose),
+      m_feedforward(feedforward),
+      m_kinematics(kinematics),
+      m_xController(std::make_unique<frc2::PIDController>(xController)),
+      m_yController(std::make_unique<frc2::PIDController>(yController)),
+      m_thetaController(
+          std::make_unique<frc::ProfiledPIDController<units::radians>>(
+              thetaController)),
+      m_maxWheelVelocity(maxWheelVelocity),
+      m_frontLeftController(
+          std::make_unique<frc2::PIDController>(frontLeftController)),
+      m_rearLeftController(
+          std::make_unique<frc2::PIDController>(rearLeftController)),
+      m_frontRightController(
+          std::make_unique<frc2::PIDController>(frontRightController)),
+      m_rearRightController(
+          std::make_unique<frc2::PIDController>(rearRightController)),
+      m_currentWheelSpeeds(currentWheelSpeeds),
+      m_outputVolts(output),
+      m_usePID(true) {
+  AddRequirements(requirements);
+}
+
+MecanumControllerCommand::MecanumControllerCommand(
+    frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+    frc::SimpleMotorFeedforward<units::meters> feedforward,
+    frc::MecanumDriveKinematics kinematics, frc2::PIDController xController,
+    frc2::PIDController yController,
+    frc::ProfiledPIDController<units::radians> thetaController,
+    units::meters_per_second_t maxWheelVelocity,
+    std::function<frc::MecanumDriveWheelSpeeds()> currentWheelSpeeds,
+    frc2::PIDController frontLeftController,
+    frc2::PIDController rearLeftController,
+    frc2::PIDController frontRightController,
+    frc2::PIDController rearRightController,
+    std::function<void(units::volt_t, units::volt_t, units::volt_t,
+                       units::volt_t)>
+        output,
+    wpi::ArrayRef<Subsystem*> requirements)
+    : m_trajectory(trajectory),
+      m_pose(pose),
+      m_feedforward(feedforward),
+      m_kinematics(kinematics),
+      m_xController(std::make_unique<frc2::PIDController>(xController)),
+      m_yController(std::make_unique<frc2::PIDController>(yController)),
+      m_thetaController(
+          std::make_unique<frc::ProfiledPIDController<units::radians>>(
+              thetaController)),
+      m_maxWheelVelocity(maxWheelVelocity),
+      m_frontLeftController(
+          std::make_unique<frc2::PIDController>(frontLeftController)),
+      m_rearLeftController(
+          std::make_unique<frc2::PIDController>(rearLeftController)),
+      m_frontRightController(
+          std::make_unique<frc2::PIDController>(frontRightController)),
+      m_rearRightController(
+          std::make_unique<frc2::PIDController>(rearRightController)),
+      m_currentWheelSpeeds(currentWheelSpeeds),
+      m_outputVolts(output),
+      m_usePID(true) {
+  AddRequirements(requirements);
+}
+
+MecanumControllerCommand::MecanumControllerCommand(
+    frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+    frc::MecanumDriveKinematics kinematics, frc2::PIDController xController,
+    frc2::PIDController yController,
+    frc::ProfiledPIDController<units::radians> thetaController,
+    units::meters_per_second_t maxWheelVelocity,
+    std::function<void(units::meters_per_second_t, units::meters_per_second_t,
+                       units::meters_per_second_t, units::meters_per_second_t)>
+        output,
+    std::initializer_list<Subsystem*> requirements)
+    : m_trajectory(trajectory),
+      m_pose(pose),
+      m_kinematics(kinematics),
+      m_xController(std::make_unique<frc2::PIDController>(xController)),
+      m_yController(std::make_unique<frc2::PIDController>(yController)),
+      m_thetaController(
+          std::make_unique<frc::ProfiledPIDController<units::radians>>(
+              thetaController)),
+      m_maxWheelVelocity(maxWheelVelocity),
+      m_outputVel(output),
+      m_usePID(false) {
+  AddRequirements(requirements);
+}
+
+MecanumControllerCommand::MecanumControllerCommand(
+    frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+    frc::MecanumDriveKinematics kinematics, frc2::PIDController xController,
+    frc2::PIDController yController,
+    frc::ProfiledPIDController<units::radians> thetaController,
+    units::meters_per_second_t maxWheelVelocity,
+    std::function<void(units::meters_per_second_t, units::meters_per_second_t,
+                       units::meters_per_second_t, units::meters_per_second_t)>
+        output,
+    wpi::ArrayRef<Subsystem*> requirements)
+    : m_trajectory(trajectory),
+      m_pose(pose),
+      m_kinematics(kinematics),
+      m_xController(std::make_unique<frc2::PIDController>(xController)),
+      m_yController(std::make_unique<frc2::PIDController>(yController)),
+      m_thetaController(
+          std::make_unique<frc::ProfiledPIDController<units::radians>>(
+              thetaController)),
+      m_maxWheelVelocity(maxWheelVelocity),
+      m_outputVel(output),
+      m_usePID(false) {
+  AddRequirements(requirements);
+}
+
+void MecanumControllerCommand::Initialize() {
+  m_prevTime = 0_s;
+  auto initialState = m_trajectory.Sample(0_s);
+
+  auto initialXVelocity =
+      initialState.velocity * initialState.pose.Rotation().Cos();
+  auto initialYVelocity =
+      initialState.velocity * initialState.pose.Rotation().Sin();
+
+  m_prevSpeeds = m_kinematics.ToWheelSpeeds(
+      frc::ChassisSpeeds{initialXVelocity, initialYVelocity, 0_rad_per_s});
+
+  m_timer.Reset();
+  m_timer.Start();
+  if (m_usePID) {
+    m_frontLeftController->Reset();
+    m_rearLeftController->Reset();
+    m_frontRightController->Reset();
+    m_rearRightController->Reset();
+  }
+}
+
+void MecanumControllerCommand::Execute() {
+  auto curTime = second_t(m_timer.Get());
+  auto dt = curTime - m_prevTime;
+
+  auto m_desiredState = m_trajectory.Sample(curTime);
+  auto m_desiredPose = m_desiredState.pose;
+
+  auto m_poseError = m_desiredPose.RelativeTo(m_pose());
+
+  auto targetXVel = meters_per_second_t(
+      m_xController->Calculate((m_pose().Translation().X().to<double>()),
+                               (m_desiredPose.Translation().X().to<double>())));
+  auto targetYVel = meters_per_second_t(
+      m_yController->Calculate((m_pose().Translation().Y().to<double>()),
+                               (m_desiredPose.Translation().Y().to<double>())));
+
+  // Profiled PID Controller only takes meters as setpoint and measurement
+  // The robot will go to the desired rotation of the final pose in the
+  // trajectory, not following the poses at individual states.
+  auto targetAngularVel = radians_per_second_t(m_thetaController->Calculate(
+      m_pose().Rotation().Radians(), m_finalPose.Rotation().Radians()));
+
+  auto vRef = m_desiredState.velocity;
+
+  targetXVel += vRef * m_poseError.Rotation().Cos();
+  targetYVel += vRef * m_poseError.Rotation().Sin();
+
+  auto targetChassisSpeeds =
+      frc::ChassisSpeeds{targetXVel, targetYVel, targetAngularVel};
+
+  auto targetWheelSpeeds = m_kinematics.ToWheelSpeeds(targetChassisSpeeds);
+
+  targetWheelSpeeds.Normalize(m_maxWheelVelocity);
+
+  auto frontLeftSpeedSetpoint = targetWheelSpeeds.frontLeft;
+  auto rearLeftSpeedSetpoint = targetWheelSpeeds.rearLeft;
+  auto frontRightSpeedSetpoint = targetWheelSpeeds.frontRight;
+  auto rearRightSpeedSetpoint = targetWheelSpeeds.rearRight;
+
+  if (m_usePID) {
+    auto frontLeftFeedforward = m_feedforward.Calculate(
+        frontLeftSpeedSetpoint,
+        (frontLeftSpeedSetpoint - m_prevSpeeds.frontLeft) / dt);
+
+    auto rearLeftFeedforward = m_feedforward.Calculate(
+        rearLeftSpeedSetpoint,
+        (rearLeftSpeedSetpoint - m_prevSpeeds.rearLeft) / dt);
+
+    auto frontRightFeedforward = m_feedforward.Calculate(
+        frontRightSpeedSetpoint,
+        (frontRightSpeedSetpoint - m_prevSpeeds.frontRight) / dt);
+
+    auto rearRightFeedforward = m_feedforward.Calculate(
+        rearRightSpeedSetpoint,
+        (rearRightSpeedSetpoint - m_prevSpeeds.rearRight) / dt);
+
+    auto frontLeftOutput = volt_t(m_frontLeftController->Calculate(
+                               m_currentWheelSpeeds().frontLeft.to<double>(),
+                               frontLeftSpeedSetpoint.to<double>())) +
+                           frontLeftFeedforward;
+    auto rearLeftOutput = volt_t(m_rearLeftController->Calculate(
+                              m_currentWheelSpeeds().rearLeft.to<double>(),
+                              rearLeftSpeedSetpoint.to<double>())) +
+                          rearLeftFeedforward;
+    auto frontRightOutput = volt_t(m_frontRightController->Calculate(
+                                m_currentWheelSpeeds().frontRight.to<double>(),
+                                frontRightSpeedSetpoint.to<double>())) +
+                            frontRightFeedforward;
+    auto rearRightOutput = volt_t(m_rearRightController->Calculate(
+                               m_currentWheelSpeeds().rearRight.to<double>(),
+                               rearRightSpeedSetpoint.to<double>())) +
+                           rearRightFeedforward;
+
+    m_outputVolts(frontLeftOutput, rearLeftOutput, frontRightOutput,
+                  rearRightOutput);
+  } else {
+    m_outputVel(frontLeftSpeedSetpoint, rearLeftSpeedSetpoint,
+                frontRightSpeedSetpoint, rearRightSpeedSetpoint);
+
+    m_prevTime = curTime;
+    m_prevSpeeds = targetWheelSpeeds;
+  }
+}
+
+void MecanumControllerCommand::End(bool interrupted) { m_timer.Stop(); }
+
+bool MecanumControllerCommand::IsFinished() {
+  return m_timer.HasPeriodPassed(m_trajectory.TotalTime());
+}
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/NotifierCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/NotifierCommand.cpp
new file mode 100644
index 0000000..1b13d75
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/NotifierCommand.cpp
@@ -0,0 +1,40 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 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(std::function<void()> toRun,
+                                 units::second_t period,
+                                 wpi::ArrayRef<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(); }
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDCommand.cpp
new file mode 100644
index 0000000..8ce3c37
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDCommand.cpp
@@ -0,0 +1,58 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 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,
+                       std::function<double()> setpointSource,
+                       std::function<void(double)> useOutput,
+                       wpi::ArrayRef<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) {}
+
+PIDCommand::PIDCommand(PIDController controller,
+                       std::function<double()> measurementSource,
+                       double setpoint, std::function<void(double)> useOutput,
+                       wpi::ArrayRef<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; }
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDSubsystem.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDSubsystem.cpp
new file mode 100644
index 0000000..14d965c
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDSubsystem.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/PIDSubsystem.h"
+
+using namespace frc2;
+
+PIDSubsystem::PIDSubsystem(PIDController controller, double initialPosition)
+    : m_controller{controller} {
+  SetSetpoint(initialPosition);
+}
+
+void PIDSubsystem::Periodic() {
+  if (m_enabled) {
+    UseOutput(m_controller.Calculate(GetMeasurement(), m_setpoint), m_setpoint);
+  }
+}
+
+void PIDSubsystem::SetSetpoint(double setpoint) { m_setpoint = setpoint; }
+
+void PIDSubsystem::Enable() {
+  m_controller.Reset();
+  m_enabled = true;
+}
+
+void PIDSubsystem::Disable() {
+  UseOutput(0, 0);
+  m_enabled = false;
+}
+
+bool PIDSubsystem::IsEnabled() { return m_enabled; }
+
+PIDController& PIDSubsystem::GetController() { return m_controller; }
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelCommandGroup.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelCommandGroup.cpp
new file mode 100644
index 0000000..557984e
--- /dev/null
+++ b/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.emplace_back(std::move(command), false);
+    } else {
+      wpi_setWPIErrorWithContext(CommandIllegalUse,
+                                 "Multiple commands in a parallel group cannot "
+                                 "require the same subsystems");
+      return;
+    }
+  }
+}
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelDeadlineGroup.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelDeadlineGroup.cpp
new file mode 100644
index 0000000..935cf8b
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelDeadlineGroup.cpp
@@ -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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#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;
+  }
+  m_finished = false;
+}
+
+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;
+      if (commandRunning.first.get() == m_deadline) {
+        m_finished = true;
+      }
+    }
+  }
+}
+
+void ParallelDeadlineGroup::End(bool interrupted) {
+  for (auto& commandRunning : m_commands) {
+    if (commandRunning.second) {
+      commandRunning.first->End(true);
+    }
+  }
+}
+
+bool ParallelDeadlineGroup::IsFinished() { return m_finished; }
+
+bool ParallelDeadlineGroup::RunsWhenDisabled() const {
+  return m_runWhenDisabled;
+}
+
+void ParallelDeadlineGroup::AddCommands(
+    std::vector<std::unique_ptr<Command>>&& commands) {
+  if (!RequireUngrouped(commands)) {
+    return;
+  }
+
+  if (!m_finished) {
+    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_back(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.emplace_back(std::move(deadline), false);
+  AddRequirements(m_deadline->GetRequirements());
+  m_runWhenDisabled &= m_deadline->RunsWhenDisabled();
+}
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp
new file mode 100644
index 0000000..466cc79
--- /dev/null
+++ b/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_back(std::move(command));
+    } else {
+      wpi_setWPIErrorWithContext(CommandIllegalUse,
+                                 "Multiple commands in a parallel group cannot "
+                                 "require the same subsystems");
+      return;
+    }
+  }
+}
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/PerpetualCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/PerpetualCommand.cpp
new file mode 100644
index 0000000..f29850b
--- /dev/null
+++ b/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); }
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/PrintCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/PrintCommand.cpp
new file mode 100644
index 0000000..e077760
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/PrintCommand.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/PrintCommand.h"
+
+#include <wpi/raw_ostream.h>
+
+using namespace frc2;
+
+PrintCommand::PrintCommand(const wpi::Twine& message)
+    : CommandHelper{[str = message.str()] { wpi::outs() << str << "\n"; }, {}} {
+}
+
+bool PrintCommand::RunsWhenDisabled() const { return true; }
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/ProxyScheduleCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/ProxyScheduleCommand.cpp
new file mode 100644
index 0000000..6f96315
--- /dev/null
+++ b/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; }
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp
new file mode 100644
index 0000000..b1608bf
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp
@@ -0,0 +1,142 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 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;
+
+RamseteCommand::RamseteCommand(
+    frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+    frc::RamseteController controller,
+    frc::SimpleMotorFeedforward<units::meters> feedforward,
+    frc::DifferentialDriveKinematics kinematics,
+    std::function<frc::DifferentialDriveWheelSpeeds()> wheelSpeeds,
+    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_feedforward(feedforward),
+      m_kinematics(kinematics),
+      m_speeds(wheelSpeeds),
+      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::SimpleMotorFeedforward<units::meters> feedforward,
+    frc::DifferentialDriveKinematics kinematics,
+    std::function<frc::DifferentialDriveWheelSpeeds()> wheelSpeeds,
+    frc2::PIDController leftController, frc2::PIDController rightController,
+    std::function<void(volt_t, volt_t)> output,
+    wpi::ArrayRef<Subsystem*> requirements)
+    : m_trajectory(trajectory),
+      m_pose(pose),
+      m_controller(controller),
+      m_feedforward(feedforward),
+      m_kinematics(kinematics),
+      m_speeds(wheelSpeeds),
+      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_kinematics(kinematics),
+      m_outputVel(output),
+      m_usePID(false) {
+  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,
+    wpi::ArrayRef<Subsystem*> requirements)
+    : m_trajectory(trajectory),
+      m_pose(pose),
+      m_controller(controller),
+      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_feedforward.Calculate(
+        targetWheelSpeeds.left,
+        (targetWheelSpeeds.left - m_prevSpeeds.left) / dt);
+
+    auto rightFeedforward = m_feedforward.Calculate(
+        targetWheelSpeeds.right,
+        (targetWheelSpeeds.right - m_prevSpeeds.right) / dt);
+
+    auto leftOutput = volt_t(m_leftController->Calculate(
+                          m_speeds().left.to<double>(),
+                          targetWheelSpeeds.left.to<double>())) +
+                      leftFeedforward;
+
+    auto rightOutput = volt_t(m_rightController->Calculate(
+                           m_speeds().right.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());
+}
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/RunCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/RunCommand.cpp
new file mode 100644
index 0000000..dff0ffe
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/RunCommand.cpp
@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 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);
+}
+
+RunCommand::RunCommand(std::function<void()> toRun,
+                       wpi::ArrayRef<Subsystem*> requirements)
+    : m_toRun{std::move(toRun)} {
+  AddRequirements(requirements);
+}
+
+void RunCommand::Execute() { m_toRun(); }
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/ScheduleCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/ScheduleCommand.cpp
new file mode 100644
index 0000000..ea1ea8d
--- /dev/null
+++ b/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; }
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/SequentialCommandGroup.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/SequentialCommandGroup.cpp
new file mode 100644
index 0000000..1aa19e4
--- /dev/null
+++ b/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));
+  }
+}
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/StartEndCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/StartEndCommand.cpp
new file mode 100644
index 0000000..ad54ae0
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/StartEndCommand.cpp
@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 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(std::function<void()> onInit,
+                                 std::function<void()> onEnd,
+                                 wpi::ArrayRef<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(); }
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/Subsystem.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/Subsystem.cpp
new file mode 100644
index 0000000..cccb55b
--- /dev/null
+++ b/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);
+}
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/SubsystemBase.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/SubsystemBase.cpp
new file mode 100644
index 0000000..9b30fec
--- /dev/null
+++ b/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);
+}
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/WaitCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/WaitCommand.cpp
new file mode 100644
index 0000000..1279d66
--- /dev/null
+++ b/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; }
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/WaitUntilCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/WaitUntilCommand.cpp
new file mode 100644
index 0000000..739a049
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/WaitUntilCommand.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/WaitUntilCommand.h"
+
+#include "frc2/Timer.h"
+
+using namespace frc2;
+
+WaitUntilCommand::WaitUntilCommand(std::function<bool()> condition)
+    : m_condition{std::move(condition)} {}
+
+WaitUntilCommand::WaitUntilCommand(units::second_t time)
+    : m_condition{[=] { return Timer::GetMatchTime() - time > 0_s; }} {}
+
+bool WaitUntilCommand::IsFinished() { return m_condition(); }
+
+bool WaitUntilCommand::RunsWhenDisabled() const { return true; }
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Button.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Button.cpp
new file mode 100644
index 0000000..d8a7372
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Button.cpp
@@ -0,0 +1,78 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 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,
+                           std::initializer_list<Subsystem*> requirements) {
+  WhenActive(std::move(toRun), requirements);
+  return *this;
+}
+
+Button Button::WhenPressed(std::function<void()> toRun,
+                           wpi::ArrayRef<Subsystem*> requirements) {
+  WhenActive(std::move(toRun), requirements);
+  return *this;
+}
+
+Button Button::WhileHeld(Command* command, bool interruptible) {
+  WhileActiveContinous(command, interruptible);
+  return *this;
+}
+
+Button Button::WhileHeld(std::function<void()> toRun,
+                         std::initializer_list<Subsystem*> requirements) {
+  WhileActiveContinous(std::move(toRun), requirements);
+  return *this;
+}
+
+Button Button::WhileHeld(std::function<void()> toRun,
+                         wpi::ArrayRef<Subsystem*> requirements) {
+  WhileActiveContinous(std::move(toRun), requirements);
+  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,
+                            std::initializer_list<Subsystem*> requirements) {
+  WhenInactive(std::move(toRun), requirements);
+  return *this;
+}
+
+Button Button::WhenReleased(std::function<void()> toRun,
+                            wpi::ArrayRef<Subsystem*> requirements) {
+  WhenInactive(std::move(toRun), requirements);
+  return *this;
+}
+
+Button Button::ToggleWhenPressed(Command* command, bool interruptible) {
+  ToggleWhenActive(command, interruptible);
+  return *this;
+}
+
+Button Button::CancelWhenPressed(Command* command) {
+  CancelWhenActive(command);
+  return *this;
+}
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp
new file mode 100644
index 0000000..5c1a8f7
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp
@@ -0,0 +1,142 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 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,
+                            std::initializer_list<Subsystem*> requirements) {
+  return WhenActive(std::move(toRun), wpi::makeArrayRef(requirements.begin(),
+                                                        requirements.end()));
+}
+
+Trigger Trigger::WhenActive(std::function<void()> toRun,
+                            wpi::ArrayRef<Subsystem*> requirements) {
+  return WhenActive(InstantCommand(std::move(toRun), requirements));
+}
+
+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,
+    std::initializer_list<Subsystem*> requirements) {
+  return WhileActiveContinous(
+      std::move(toRun),
+      wpi::makeArrayRef(requirements.begin(), requirements.end()));
+}
+
+Trigger Trigger::WhileActiveContinous(std::function<void()> toRun,
+                                      wpi::ArrayRef<Subsystem*> requirements) {
+  return WhileActiveContinous(InstantCommand(std::move(toRun), requirements));
+}
+
+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,
+                              std::initializer_list<Subsystem*> requirements) {
+  return WhenInactive(std::move(toRun), wpi::makeArrayRef(requirements.begin(),
+                                                          requirements.end()));
+}
+
+Trigger Trigger::WhenInactive(std::function<void()> toRun,
+                              wpi::ArrayRef<Subsystem*> requirements) {
+  return WhenInactive(InstantCommand(std::move(toRun), requirements));
+}
+
+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;
+}
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/Command.h b/wpilibNewCommands/src/main/native/include/frc2/command/Command.h
new file mode 100644
index 0000000..e50f09e
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/Command.h
@@ -0,0 +1,270 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 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 <functional>
+#include <initializer_list>
+#include <memory>
+#include <string>
+
+#include <frc/ErrorBase.h>
+#include <units/units.h>
+#include <wpi/ArrayRef.h>
+#include <wpi/Demangle.h>
+#include <wpi/SmallSet.h>
+
+#include "frc2/command/Subsystem.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
+   * @param requirements the required subsystems
+   * @return the decorated command
+   */
+  SequentialCommandGroup BeforeStarting(
+      std::function<void()> toRun,
+      std::initializer_list<Subsystem*> requirements) &&;
+
+  /**
+   * Decorates this command with a runnable to run before this command starts.
+   *
+   * @param toRun the Runnable to run
+   * @param requirements the required subsystems
+   * @return the decorated command
+   */
+  SequentialCommandGroup BeforeStarting(
+      std::function<void()> toRun,
+      wpi::ArrayRef<Subsystem*> requirements = {}) &&;
+
+  /**
+   * Decorates this command with a runnable to run after the command finishes.
+   *
+   * @param toRun the Runnable to run
+   * @param requirements the required subsystems
+   * @return the decorated command
+   */
+  SequentialCommandGroup AndThen(
+      std::function<void()> toRun,
+      std::initializer_list<Subsystem*> requirements) &&;
+
+  /**
+   * Decorates this command with a runnable to run after the command finishes.
+   *
+   * @param toRun the Runnable to run
+   * @param requirements the required subsystems
+   * @return the decorated command
+   */
+  SequentialCommandGroup AndThen(
+      std::function<void()> toRun,
+      wpi::ArrayRef<Subsystem*> requirements = {}) &&;
+
+  /**
+   * 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
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/CommandBase.h b/wpilibNewCommands/src/main/native/include/frc2/command/CommandBase.h
new file mode 100644
index 0000000..481a51a
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/CommandBase.h
@@ -0,0 +1,81 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 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 <initializer_list>
+#include <string>
+
+#include <frc/smartdashboard/Sendable.h>
+#include <frc/smartdashboard/SendableHelper.h>
+#include <wpi/ArrayRef.h>
+#include <wpi/SmallSet.h>
+#include <wpi/Twine.h>
+
+#include "frc2/command/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);
+
+  /**
+   * Adds the specified requirements to the command.
+   *
+   * @param requirements the requirements to add
+   */
+  void AddRequirements(wpi::ArrayRef<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
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/CommandGroupBase.h b/wpilibNewCommands/src/main/native/include/frc2/command/CommandGroupBase.h
new file mode 100644
index 0000000..8b04b4c
--- /dev/null
+++ b/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 <initializer_list>
+#include <memory>
+#include <vector>
+
+#include <wpi/ArrayRef.h>
+
+#include "frc2/command/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
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/CommandHelper.h b/wpilibNewCommands/src/main/native/include/frc2/command/CommandHelper.h
new file mode 100644
index 0000000..f78a848
--- /dev/null
+++ b/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 "frc2/command/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
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h b/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h
new file mode 100644
index 0000000..f1d0917
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h
@@ -0,0 +1,342 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 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 <initializer_list>
+#include <memory>
+#include <utility>
+
+#include <frc/ErrorBase.h>
+#include <frc/WPIErrors.h>
+#include <frc/smartdashboard/Sendable.h>
+#include <frc/smartdashboard/SendableHelper.h>
+#include <wpi/ArrayRef.h>
+#include <wpi/FunctionExtras.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();
+
+  ~CommandScheduler();
+  CommandScheduler(const CommandScheduler&) = delete;
+  CommandScheduler& operator=(const CommandScheduler&) = delete;
+
+  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 RegisterSubsystem(wpi::ArrayRef<Subsystem*> subsystems);
+
+  void UnregisterSubsystem(std::initializer_list<Subsystem*> subsystems);
+  void UnregisterSubsystem(wpi::ArrayRef<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;
+    }
+    SetDefaultCommandImpl(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();
+
+  void SetDefaultCommandImpl(Subsystem* subsystem,
+                             std::unique_ptr<Command> command);
+
+  class Impl;
+  std::unique_ptr<Impl> m_impl;
+
+  friend class CommandTestBase;
+};
+}  // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/CommandState.h b/wpilibNewCommands/src/main/native/include/frc2/command/CommandState.h
new file mode 100644
index 0000000..41fc5d0
--- /dev/null
+++ b/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
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/ConditionalCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/ConditionalCommand.h
new file mode 100644
index 0000000..af9f113
--- /dev/null
+++ b/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 <functional>
+#include <memory>
+#include <utility>
+
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandGroupBase.h"
+#include "frc2/command/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
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/FunctionalCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/FunctionalCommand.h
new file mode 100644
index 0000000..eb88b5e
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/FunctionalCommand.h
@@ -0,0 +1,78 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 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 <functional>
+#include <initializer_list>
+
+#include <wpi/ArrayRef.h>
+
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/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,
+                    std::initializer_list<Subsystem*> requirements);
+
+  /**
+   * 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,
+                    wpi::ArrayRef<Subsystem*> requirements = {});
+
+  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
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/InstantCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/InstantCommand.h
new file mode 100644
index 0000000..3f29b17
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/InstantCommand.h
@@ -0,0 +1,63 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 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 <functional>
+#include <initializer_list>
+
+#include <wpi/ArrayRef.h>
+
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/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);
+
+  /**
+   * 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,
+                 wpi::ArrayRef<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
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/MecanumControllerCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/MecanumControllerCommand.h
new file mode 100644
index 0000000..f667e5f
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/MecanumControllerCommand.h
@@ -0,0 +1,266 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 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 <cmath>
+#include <functional>
+#include <initializer_list>
+#include <memory>
+
+#include <frc/controller/PIDController.h>
+#include <frc/controller/ProfiledPIDController.h>
+#include <frc/controller/SimpleMotorFeedforward.h>
+#include <frc/geometry/Pose2d.h>
+#include <frc/kinematics/ChassisSpeeds.h>
+#include <frc/kinematics/MecanumDriveKinematics.h>
+#include <frc/kinematics/MecanumDriveWheelSpeeds.h>
+#include <frc/trajectory/Trajectory.h>
+#include <units/units.h>
+#include <wpi/ArrayRef.h>
+
+#include "CommandBase.h"
+#include "CommandHelper.h"
+#include "frc2/Timer.h"
+
+#pragma once
+
+namespace frc2 {
+/**
+ * A command that uses two PID controllers ({@link PIDController}) and a
+ * ProfiledPIDController ({@link ProfiledPIDController}) to follow a trajectory
+ * {@link Trajectory} with a mecanum drive.
+ *
+ * <p>The command handles trajectory-following,
+ * Velocity 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 PID controllers.
+ *
+ * <p>The robot angle controller does not follow the angle given by
+ * the trajectory but rather goes to the angle given in the final state of the
+ * trajectory.
+ */
+class MecanumControllerCommand
+    : public CommandHelper<CommandBase, MecanumControllerCommand> {
+ public:
+  /**
+   * Constructs a new MecanumControllerCommand that when executed will follow
+   * the provided trajectory. PID control and feedforward are handled
+   * internally. Outputs are scaled from -12 to 12 as a voltage output to the
+   * motor.
+   *
+   * <p>Note: The controllers 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.
+   *
+   * <p>Note 2: The rotation controller will calculate the rotation based on the
+   * final pose in the trajectory, not the poses at each time step.
+   *
+   * @param trajectory           The trajectory to follow.
+   * @param pose                 A function that supplies the robot pose,
+   *                             provided by the odometry class.
+   * @param feedforward          The feedforward to use for the drivetrain.
+   * @param kinematics           The kinematics for the robot drivetrain.
+   * @param xController          The Trajectory Tracker PID controller
+   *                             for the robot's x position.
+   * @param yController          The Trajectory Tracker PID controller
+   *                             for the robot's y position.
+   * @param thetaController      The Trajectory Tracker PID controller
+   *                             for angle for the robot.
+   * @param maxWheelVelocity     The maximum velocity of a drivetrain wheel.
+   * @param frontLeftController  The front left wheel velocity PID.
+   * @param rearLeftController   The rear left wheel velocity PID.
+   * @param frontRightController The front right wheel velocity PID.
+   * @param rearRightController  The rear right wheel velocity PID.
+   * @param currentWheelSpeeds   A MecanumDriveWheelSpeeds object containing
+   *                             the current wheel speeds.
+   * @param output               The output of the velocity PIDs.
+   * @param requirements         The subsystems to require.
+   */
+  MecanumControllerCommand(
+      frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+      frc::SimpleMotorFeedforward<units::meters> feedforward,
+      frc::MecanumDriveKinematics kinematics, frc2::PIDController xController,
+      frc2::PIDController yController,
+      frc::ProfiledPIDController<units::radians> thetaController,
+      units::meters_per_second_t maxWheelVelocity,
+      std::function<frc::MecanumDriveWheelSpeeds()> currentWheelSpeeds,
+      frc2::PIDController frontLeftController,
+      frc2::PIDController rearLeftController,
+      frc2::PIDController frontRightController,
+      frc2::PIDController rearRightController,
+      std::function<void(units::volt_t, units::volt_t, units::volt_t,
+                         units::volt_t)>
+          output,
+      std::initializer_list<Subsystem*> requirements);
+
+  /**
+   * Constructs a new MecanumControllerCommand that when executed will follow
+   * the provided trajectory. PID control and feedforward are handled
+   * internally. Outputs are scaled from -12 to 12 as a voltage output to the
+   * motor.
+   *
+   * <p>Note: The controllers 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.
+   *
+   * <p>Note 2: The rotation controller will calculate the rotation based on the
+   * final pose in the trajectory, not the poses at each time step.
+   *
+   * @param trajectory           The trajectory to follow.
+   * @param pose                 A function that supplies the robot pose,
+   *                             provided by the odometry class.
+   * @param feedforward          The feedforward to use for the drivetrain.
+   * @param kinematics           The kinematics for the robot drivetrain.
+   * @param xController          The Trajectory Tracker PID controller
+   *                             for the robot's x position.
+   * @param yController          The Trajectory Tracker PID controller
+   *                             for the robot's y position.
+   * @param thetaController      The Trajectory Tracker PID controller
+   *                             for angle for the robot.
+   * @param maxWheelVelocity     The maximum velocity of a drivetrain wheel.
+   * @param frontLeftController  The front left wheel velocity PID.
+   * @param rearLeftController   The rear left wheel velocity PID.
+   * @param frontRightController The front right wheel velocity PID.
+   * @param rearRightController  The rear right wheel velocity PID.
+   * @param currentWheelSpeeds   A MecanumDriveWheelSpeeds object containing
+   *                             the current wheel speeds.
+   * @param output               The output of the velocity PIDs.
+   * @param requirements         The subsystems to require.
+   */
+  MecanumControllerCommand(
+      frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+      frc::SimpleMotorFeedforward<units::meters> feedforward,
+      frc::MecanumDriveKinematics kinematics, frc2::PIDController xController,
+      frc2::PIDController yController,
+      frc::ProfiledPIDController<units::radians> thetaController,
+      units::meters_per_second_t maxWheelVelocity,
+      std::function<frc::MecanumDriveWheelSpeeds()> currentWheelSpeeds,
+      frc2::PIDController frontLeftController,
+      frc2::PIDController rearLeftController,
+      frc2::PIDController frontRightController,
+      frc2::PIDController rearRightController,
+      std::function<void(units::volt_t, units::volt_t, units::volt_t,
+                         units::volt_t)>
+          output,
+      wpi::ArrayRef<Subsystem*> requirements = {});
+
+  /**
+   * Constructs a new MecanumControllerCommand that when executed will follow
+   * the provided trajectory. The user should implement a velocity PID on the
+   * desired output wheel velocities.
+   *
+   * <p>Note: The controllers 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 non-stationary end-states.
+   *
+   * <p>Note2: The rotation controller will calculate the rotation based on the
+   * final pose in the trajectory, not the poses at each time step.
+   *
+   * @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 kinematics       The kinematics for the robot drivetrain.
+   * @param xController      The Trajectory Tracker PID controller
+   *                         for the robot's x position.
+   * @param yController      The Trajectory Tracker PID controller
+   *                         for the robot's y position.
+   * @param thetaController  The Trajectory Tracker PID controller
+   *                         for angle for the robot.
+   * @param maxWheelVelocity The maximum velocity of a drivetrain wheel.
+   * @param output           The output of the position PIDs.
+   * @param requirements     The subsystems to require.
+   */
+  MecanumControllerCommand(
+      frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+      frc::MecanumDriveKinematics kinematics, frc2::PIDController xController,
+      frc2::PIDController yController,
+      frc::ProfiledPIDController<units::radians> thetaController,
+      units::meters_per_second_t maxWheelVelocity,
+      std::function<void(units::meters_per_second_t, units::meters_per_second_t,
+                         units::meters_per_second_t,
+                         units::meters_per_second_t)>
+          output,
+      std::initializer_list<Subsystem*> requirements);
+
+  /**
+   * Constructs a new MecanumControllerCommand that when executed will follow
+   * the provided trajectory. The user should implement a velocity PID on the
+   * desired output wheel velocities.
+   *
+   * <p>Note: The controllers 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 non-stationary end-states.
+   *
+   * <p>Note2: The rotation controller will calculate the rotation based on the
+   * final pose in the trajectory, not the poses at each time step.
+   *
+   * @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 kinematics       The kinematics for the robot drivetrain.
+   * @param xController      The Trajectory Tracker PID controller
+   *                         for the robot's x position.
+   * @param yController      The Trajectory Tracker PID controller
+   *                         for the robot's y position.
+   * @param thetaController  The Trajectory Tracker PID controller
+   *                         for angle for the robot.
+   * @param maxWheelVelocity The maximum velocity of a drivetrain wheel.
+   * @param output           The output of the position PIDs.
+   * @param requirements     The subsystems to require.
+   */
+  MecanumControllerCommand(
+      frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+      frc::MecanumDriveKinematics kinematics, frc2::PIDController xController,
+      frc2::PIDController yController,
+      frc::ProfiledPIDController<units::radians> thetaController,
+      units::meters_per_second_t maxWheelVelocity,
+      std::function<void(units::meters_per_second_t, units::meters_per_second_t,
+                         units::meters_per_second_t,
+                         units::meters_per_second_t)>
+          output,
+      wpi::ArrayRef<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::SimpleMotorFeedforward<units::meters> m_feedforward;
+  frc::MecanumDriveKinematics m_kinematics;
+  std::unique_ptr<frc2::PIDController> m_xController;
+  std::unique_ptr<frc2::PIDController> m_yController;
+  std::unique_ptr<frc::ProfiledPIDController<units::radians>> m_thetaController;
+  const units::meters_per_second_t m_maxWheelVelocity;
+  std::unique_ptr<frc2::PIDController> m_frontLeftController;
+  std::unique_ptr<frc2::PIDController> m_rearLeftController;
+  std::unique_ptr<frc2::PIDController> m_frontRightController;
+  std::unique_ptr<frc2::PIDController> m_rearRightController;
+  std::function<frc::MecanumDriveWheelSpeeds()> m_currentWheelSpeeds;
+  std::function<void(units::meters_per_second_t, units::meters_per_second_t,
+                     units::meters_per_second_t, units::meters_per_second_t)>
+      m_outputVel;
+  std::function<void(units::volt_t, units::volt_t, units::volt_t,
+                     units::volt_t)>
+      m_outputVolts;
+
+  bool m_usePID;
+  frc2::Timer m_timer;
+  frc::MecanumDriveWheelSpeeds m_prevSpeeds;
+  units::second_t m_prevTime;
+  frc::Pose2d m_finalPose;
+};
+}  // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/NotifierCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/NotifierCommand.h
new file mode 100644
index 0000000..59af028
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/NotifierCommand.h
@@ -0,0 +1,66 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 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 <functional>
+#include <initializer_list>
+
+#include <frc/Notifier.h>
+#include <units/units.h>
+#include <wpi/ArrayRef.h>
+
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/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);
+
+  /**
+   * 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,
+                  wpi::ArrayRef<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
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/PIDCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/PIDCommand.h
new file mode 100644
index 0000000..8653313
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/PIDCommand.h
@@ -0,0 +1,114 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 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 <functional>
+#include <initializer_list>
+
+#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.
+   *
+   * @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,
+             wpi::ArrayRef<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);
+
+  /**
+   * 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,
+             wpi::ArrayRef<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
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/PIDSubsystem.h b/wpilibNewCommands/src/main/native/include/frc2/command/PIDSubsystem.h
new file mode 100644
index 0000000..3a6df88
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/PIDSubsystem.h
@@ -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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#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
+   * @param initialPosition the initial setpoint of the subsystem
+   */
+  explicit PIDSubsystem(PIDController controller, double initialPosition = 0);
+
+  void Periodic() override;
+
+  /**
+   * Sets the setpoint for the subsystem.
+   *
+   * @param setpoint the setpoint for the subsystem
+   */
+  void SetSetpoint(double setpoint);
+
+  /**
+   * Enables the PID control.  Resets the controller.
+   */
+  virtual void Enable();
+
+  /**
+   * Disables the PID control.  Sets output to zero.
+   */
+  virtual void Disable();
+
+  /**
+   * Returns whether the controller is enabled.
+   *
+   * @return Whether the controller is enabled.
+   */
+  bool IsEnabled();
+
+  /**
+   * Returns the PIDController.
+   *
+   * @return The controller.
+   */
+  PIDController& GetController();
+
+ protected:
+  PIDController m_controller;
+  bool m_enabled{false};
+
+  /**
+   * Returns the measurement of the process variable used by the PIDController.
+   *
+   * @return the measurement of the process variable
+   */
+  virtual double GetMeasurement() = 0;
+
+  /**
+   * Uses the output from the PIDController.
+   *
+   * @param output the output of the PIDController
+   * @param setpoint the setpoint of the PIDController (for feedforward)
+   */
+  virtual void UseOutput(double output, double setpoint) = 0;
+
+ private:
+  double m_setpoint{0};
+};
+}  // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/ParallelCommandGroup.h b/wpilibNewCommands/src/main/native/include/frc2/command/ParallelCommandGroup.h
new file mode 100644
index 0000000..5b0f0da
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/ParallelCommandGroup.h
@@ -0,0 +1,99 @@
+/*----------------------------------------------------------------------------*/
+/* 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 <vector>
+
+#include "frc2/command/CommandGroupBase.h"
+#include "frc2/command/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::vector<std::pair<std::unique_ptr<Command>, bool>> m_commands;
+  bool m_runWhenDisabled{true};
+  bool isRunning = false;
+};
+}  // namespace frc2
+
+#ifdef _WIN32
+#pragma warning(pop)
+#endif
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/ParallelDeadlineGroup.h b/wpilibNewCommands/src/main/native/include/frc2/command/ParallelDeadlineGroup.h
new file mode 100644
index 0000000..4debe75
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/ParallelDeadlineGroup.h
@@ -0,0 +1,110 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#ifdef _WIN32
+#pragma warning(push)
+#pragma warning(disable : 4521)
+#endif
+
+#include <memory>
+#include <utility>
+#include <vector>
+
+#include "frc2/command/CommandGroupBase.h"
+#include "frc2/command/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::vector<std::pair<std::unique_ptr<Command>, bool>> m_commands;
+  Command* m_deadline;
+  bool m_runWhenDisabled{true};
+  bool m_finished{true};
+};
+}  // namespace frc2
+
+#ifdef _WIN32
+#pragma warning(pop)
+#endif
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/ParallelRaceGroup.h b/wpilibNewCommands/src/main/native/include/frc2/command/ParallelRaceGroup.h
new file mode 100644
index 0000000..167dee4
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/ParallelRaceGroup.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
+
+#ifdef _WIN32
+#pragma warning(push)
+#pragma warning(disable : 4521)
+#endif
+
+#include <memory>
+#include <utility>
+#include <vector>
+
+#include "frc2/command/CommandGroupBase.h"
+#include "frc2/command/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::vector<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
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/PerpetualCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/PerpetualCommand.h
new file mode 100644
index 0000000..4a93704
--- /dev/null
+++ b/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 "frc2/command/CommandBase.h"
+#include "frc2/command/CommandGroupBase.h"
+#include "frc2/command/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
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/PrintCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/PrintCommand.h
new file mode 100644
index 0000000..7b706a0
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/PrintCommand.h
@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <wpi/Twine.h>
+
+#include "frc2/command/CommandHelper.h"
+#include "frc2/command/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
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h
new file mode 100644
index 0000000..b685230
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h
@@ -0,0 +1,224 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 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 <functional>
+#include <initializer_list>
+#include <utility>
+
+#include <frc/controller/ProfiledPIDController.h>
+#include <units/units.h>
+#include <wpi/ArrayRef.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<Distance>
+ */
+template <class Distance>
+class ProfiledPIDCommand
+    : public CommandHelper<CommandBase, ProfiledPIDCommand<Distance>> {
+  using Distance_t = units::unit_t<Distance>;
+  using Velocity =
+      units::compound_unit<Distance, units::inverse<units::seconds>>;
+  using Velocity_t = units::unit_t<Velocity>;
+  using State = typename frc::TrapezoidProfile<Distance>::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<Distance> controller,
+                     std::function<Distance_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)} {
+    this->AddRequirements(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<Distance> controller,
+                     std::function<Distance_t()> measurementSource,
+                     std::function<State()> goalSource,
+                     std::function<void(double, State)> useOutput,
+                     wpi::ArrayRef<Subsystem*> requirements = {})
+      : m_controller{controller},
+        m_measurement{std::move(measurementSource)},
+        m_goal{std::move(goalSource)},
+        m_useOutput{std::move(useOutput)} {
+    this->AddRequirements(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<Distance> controller,
+                     std::function<Distance_t()> measurementSource,
+                     std::function<Distance_t()> goalSource,
+                     std::function<void(double, State)> useOutput,
+                     std::initializer_list<Subsystem*> requirements)
+      : ProfiledPIDCommand(controller, measurementSource,
+                           [&goalSource]() {
+                             return State{goalSource(), Velocity_t{0}};
+                           },
+                           useOutput, 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<Distance> controller,
+                     std::function<Distance_t()> measurementSource,
+                     std::function<Distance_t()> goalSource,
+                     std::function<void(double, State)> useOutput,
+                     wpi::ArrayRef<Subsystem*> requirements = {})
+      : ProfiledPIDCommand(controller, measurementSource,
+                           [&goalSource]() {
+                             return State{goalSource(), Velocity_t{0}};
+                           },
+                           useOutput, 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<Distance> controller,
+                     std::function<Distance_t()> measurementSource, State goal,
+                     std::function<void(double, State)> useOutput,
+                     std::initializer_list<Subsystem*> requirements)
+      : ProfiledPIDCommand(controller, measurementSource,
+                           [goal] { return goal; }, useOutput, 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<Distance> controller,
+                     std::function<Distance_t()> measurementSource, State goal,
+                     std::function<void(double, State)> useOutput,
+                     wpi::ArrayRef<Subsystem*> requirements = {})
+      : ProfiledPIDCommand(controller, measurementSource,
+                           [goal] { return goal; }, useOutput, 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<Distance> controller,
+                     std::function<Distance_t()> measurementSource,
+                     Distance_t goal,
+                     std::function<void(double, State)> useOutput,
+                     std::initializer_list<Subsystem*> requirements)
+      : ProfiledPIDCommand(controller, measurementSource,
+                           [goal] { return goal; }, useOutput, 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<Distance> controller,
+                     std::function<Distance_t()> measurementSource,
+                     Distance_t goal,
+                     std::function<void(double, State)> useOutput,
+                     wpi::ArrayRef<Subsystem*> requirements = {})
+      : ProfiledPIDCommand(controller, measurementSource,
+                           [goal] { return goal; }, useOutput, requirements) {}
+
+  ProfiledPIDCommand(ProfiledPIDCommand&& other) = default;
+
+  ProfiledPIDCommand(const ProfiledPIDCommand& other) = default;
+
+  void Initialize() override { m_controller.Reset(m_measurement()); }
+
+  void Execute() override {
+    m_useOutput(m_controller.Calculate(m_measurement(), m_goal()),
+                m_controller.GetSetpoint());
+  }
+
+  void End(bool interrupted) override {
+    m_useOutput(0, State{Distance_t(0), Velocity_t(0)});
+  }
+
+  /**
+   * Returns the ProfiledPIDController used by the command.
+   *
+   * @return The ProfiledPIDController
+   */
+  frc::ProfiledPIDController<Distance>& GetController() { return m_controller; }
+
+ protected:
+  frc::ProfiledPIDController<Distance> m_controller;
+  std::function<Distance_t()> m_measurement;
+  std::function<State()> m_goal;
+  std::function<void(double, State)> m_useOutput;
+};
+}  // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h b/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h
new file mode 100644
index 0000000..919ed18
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h
@@ -0,0 +1,118 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 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/ProfiledPIDController.h>
+#include <units/units.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
+ */
+template <class Distance>
+class ProfiledPIDSubsystem : public SubsystemBase {
+  using Distance_t = units::unit_t<Distance>;
+  using Velocity =
+      units::compound_unit<Distance, units::inverse<units::seconds>>;
+  using Velocity_t = units::unit_t<Velocity>;
+  using State = typename frc::TrapezoidProfile<Distance>::State;
+
+ public:
+  /**
+   * Creates a new ProfiledPIDSubsystem.
+   *
+   * @param controller the ProfiledPIDController to use
+   * @param initialPosition the initial goal position of the subsystem
+   */
+  explicit ProfiledPIDSubsystem(frc::ProfiledPIDController<Distance> controller,
+                                Distance_t initialPosition = Distance_t{0})
+      : m_controller{controller} {
+    SetGoal(initialPosition);
+  }
+
+  void Periodic() override {
+    if (m_enabled) {
+      UseOutput(m_controller.Calculate(GetMeasurement(), m_goal),
+                m_controller.GetSetpoint());
+    }
+  }
+
+  /**
+   * Sets the goal state for the subsystem.
+   *
+   * @param goal The goal state for the subsystem's motion profile.
+   */
+  void SetGoal(State goal) { m_goal = goal; }
+
+  /**
+   * Sets the goal state for the subsystem.  Goal velocity assumed to be zero.
+   *
+   * @param goal The goal position for the subsystem's motion profile.
+   */
+  void SetGoal(Distance_t goal) { m_goal = State{goal, Velocity_t(0)}; }
+
+  /**
+   * Enables the PID control. Resets the controller.
+   */
+  virtual void Enable() {
+    m_controller.Reset(GetMeasurement());
+    m_enabled = true;
+  }
+
+  /**
+   * Disables the PID control.  Sets output to zero.
+   */
+  virtual void Disable() {
+    UseOutput(0, State{Distance_t(0), Velocity_t(0)});
+    m_enabled = false;
+  }
+
+  /**
+   * Returns whether the controller is enabled.
+   *
+   * @return Whether the controller is enabled.
+   */
+  bool IsEnabled() { return m_enabled; }
+
+  /**
+   * Returns the ProfiledPIDController.
+   *
+   * @return The controller.
+   */
+  frc::ProfiledPIDController<Distance>& GetController() { return m_controller; }
+
+ protected:
+  frc::ProfiledPIDController<Distance> m_controller;
+  bool m_enabled{false};
+
+  /**
+   * Returns the measurement of the process variable used by the
+   * ProfiledPIDController.
+   *
+   * @return the measurement of the process variable
+   */
+  virtual Distance_t GetMeasurement() = 0;
+
+  /**
+   * Uses the output from the ProfiledPIDController.
+   *
+   * @param output the output of the ProfiledPIDController
+   * @param setpoint the setpoint state of the ProfiledPIDController, for
+   * feedforward
+   */
+  virtual void UseOutput(double output, State setpoint) = 0;
+
+ private:
+  State m_goal;
+};
+}  // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/ProxyScheduleCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/ProxyScheduleCommand.h
new file mode 100644
index 0000000..ef5204f
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/ProxyScheduleCommand.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 <wpi/ArrayRef.h>
+#include <wpi/SmallVector.h>
+
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.h"
+#include "frc2/command/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
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h
new file mode 100644
index 0000000..580bc57
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h
@@ -0,0 +1,195 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 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 <functional>
+#include <initializer_list>
+#include <memory>
+
+#include <frc/controller/PIDController.h>
+#include <frc/controller/RamseteController.h>
+#include <frc/controller/SimpleMotorFeedforward.h>
+#include <frc/geometry/Pose2d.h>
+#include <frc/kinematics/DifferentialDriveKinematics.h>
+#include <frc/trajectory/Trajectory.h>
+#include <units/units.h>
+#include <wpi/ArrayRef.h>
+
+#include "frc2/Timer.h"
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.h"
+
+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> {
+ public:
+  /**
+   * Constructs a new RamseteCommand that, when executed, will follow the
+   * provided trajectory. PID control and feedforward are handled internally,
+   * and outputs are scaled -12 to 12 representing units of volts.
+   *
+   * <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 feedforward     A component for calculating the feedforward for the
+   * drive.
+   * @param kinematics      The kinematics for the robot drivetrain.
+   * @param wheelSpeeds     A function that supplies the speeds of the left
+   * and right sides 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,
+                 frc::SimpleMotorFeedforward<units::meters> feedforward,
+                 frc::DifferentialDriveKinematics kinematics,
+                 std::function<frc::DifferentialDriveWheelSpeeds()> wheelSpeeds,
+                 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. PID control and feedforward are handled internally,
+   * and outputs are scaled -12 to 12 representing units of volts.
+   *
+   * <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 feedforward     A component for calculating the feedforward for the
+   * drive.
+   * @param kinematics      The kinematics for the robot drivetrain.
+   * @param wheelSpeeds     A function that supplies the speeds of the left
+   * and right sides 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,
+                 frc::SimpleMotorFeedforward<units::meters> feedforward,
+                 frc::DifferentialDriveKinematics kinematics,
+                 std::function<frc::DifferentialDriveWheelSpeeds()> wheelSpeeds,
+                 frc2::PIDController leftController,
+                 frc2::PIDController rightController,
+                 std::function<void(units::volt_t, units::volt_t)> output,
+                 wpi::ArrayRef<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
+   * wheel speeds.
+   * @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);
+
+  /**
+   * 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
+   * wheel speeds.
+   * @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,
+                 wpi::ArrayRef<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;
+  frc::SimpleMotorFeedforward<units::meters> m_feedforward;
+  frc::DifferentialDriveKinematics m_kinematics;
+  std::function<frc::DifferentialDriveWheelSpeeds()> m_speeds;
+  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
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/RunCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/RunCommand.h
new file mode 100644
index 0000000..2036230
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/RunCommand.h
@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 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 <functional>
+#include <initializer_list>
+
+#include <wpi/ArrayRef.h>
+
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/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);
+
+  /**
+   * 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,
+             wpi::ArrayRef<Subsystem*> requirements = {});
+
+  RunCommand(RunCommand&& other) = default;
+
+  RunCommand(const RunCommand& other) = default;
+
+  void Execute();
+
+ protected:
+  std::function<void()> m_toRun;
+};
+}  // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/ScheduleCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/ScheduleCommand.h
new file mode 100644
index 0000000..c2824ca
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/ScheduleCommand.h
@@ -0,0 +1,47 @@
+/*----------------------------------------------------------------------------*/
+/* 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>
+
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.h"
+#include "frc2/command/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
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/SelectCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/SelectCommand.h
new file mode 100644
index 0000000..e1074f4
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/SelectCommand.h
@@ -0,0 +1,154 @@
+/*----------------------------------------------------------------------------*/
+/* 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 <type_traits>
+#include <unordered_map>
+#include <utility>
+#include <vector>
+
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandGroupBase.h"
+#include "frc2/command/PrintCommand.h"
+
+namespace frc2 {
+/**
+ * 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.
+ */
+template <typename Key>
+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
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/SequentialCommandGroup.h b/wpilibNewCommands/src/main/native/include/frc2/command/SequentialCommandGroup.h
new file mode 100644
index 0000000..c49356e
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/SequentialCommandGroup.h
@@ -0,0 +1,105 @@
+/*----------------------------------------------------------------------------*/
+/* 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 <type_traits>
+#include <utility>
+#include <vector>
+
+#include <frc/ErrorBase.h>
+#include <frc/WPIErrors.h>
+#include <wpi/ArrayRef.h>
+
+#include "frc2/command/CommandGroupBase.h"
+#include "frc2/command/CommandHelper.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
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/SetUtilities.h b/wpilibNewCommands/src/main/native/include/frc2/command/SetUtilities.h
new file mode 100644
index 0000000..d21f572
--- /dev/null
+++ b/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
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/StartEndCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/StartEndCommand.h
new file mode 100644
index 0000000..c0c9096
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/StartEndCommand.h
@@ -0,0 +1,62 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 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 <functional>
+#include <initializer_list>
+
+#include <wpi/ArrayRef.h>
+
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/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);
+
+  /**
+   * 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,
+                  wpi::ArrayRef<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
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h b/wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h
new file mode 100644
index 0000000..687510d
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h
@@ -0,0 +1,89 @@
+/*----------------------------------------------------------------------------*/
+/* 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 <type_traits>
+#include <utility>
+
+#include "frc2/command/CommandScheduler.h"
+
+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
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/SubsystemBase.h b/wpilibNewCommands/src/main/native/include/frc2/command/SubsystemBase.h
new file mode 100644
index 0000000..d75ad0d
--- /dev/null
+++ b/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 <string>
+
+#include <frc/smartdashboard/Sendable.h>
+#include <frc/smartdashboard/SendableHelper.h>
+
+#include "frc2/command/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
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.h
new file mode 100644
index 0000000..38ca60c
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.h
@@ -0,0 +1,157 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 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 <cmath>
+#include <functional>
+#include <initializer_list>
+#include <memory>
+
+#include <frc/controller/PIDController.h>
+#include <frc/controller/ProfiledPIDController.h>
+#include <frc/geometry/Pose2d.h>
+#include <frc/kinematics/ChassisSpeeds.h>
+#include <frc/kinematics/SwerveDriveKinematics.h>
+#include <frc/kinematics/SwerveModuleState.h>
+#include <frc/trajectory/Trajectory.h>
+#include <units/units.h>
+#include <wpi/ArrayRef.h>
+
+#include "CommandBase.h"
+#include "CommandHelper.h"
+#include "frc2/Timer.h"
+
+#pragma once
+
+namespace frc2 {
+
+/**
+ * A command that uses two PID controllers ({@link PIDController}) and a
+ * ProfiledPIDController ({@link ProfiledPIDController}) to follow a trajectory
+ * {@link Trajectory} with a swerve drive.
+ *
+ * <p>The command handles trajectory-following, Velocity 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 module states from the position PID controllers.
+ *
+ * <p>The robot angle controller does not follow the angle given by
+ * the trajectory but rather goes to the angle given in the final state of the
+ * trajectory.
+ */
+template <size_t NumModules>
+class SwerveControllerCommand
+    : public CommandHelper<CommandBase, SwerveControllerCommand<NumModules>> {
+  using voltsecondspermeter =
+      units::compound_unit<units::voltage::volt, units::second,
+                           units::inverse<units::meter>>;
+  using voltsecondssquaredpermeter =
+      units::compound_unit<units::voltage::volt, units::squared<units::second>,
+                           units::inverse<units::meter>>;
+
+ public:
+  /**
+   * Constructs a new SwerveControllerCommand that when executed will follow the
+   * provided trajectory. This command will not return output voltages but
+   * rather raw module states from the position controllers which need to be put
+   * into a velocity PID.
+   *
+   * <p>Note: The controllers 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.
+   *
+   * <p>Note 2: The rotation controller will calculate the rotation based on the
+   * final pose in the trajectory, not the poses at each time step.
+   *
+   * @param trajectory      The trajectory to follow.
+   * @param pose            A function that supplies the robot pose,
+   *                        provided by the odometry class.
+   * @param kinematics      The kinematics for the robot drivetrain.
+   * @param xController     The Trajectory Tracker PID controller
+   *                        for the robot's x position.
+   * @param yController     The Trajectory Tracker PID controller
+   *                        for the robot's y position.
+   * @param thetaController The Trajectory Tracker PID controller
+   *                        for angle for the robot.
+   * @param output          The raw output module states from the
+   *                        position controllers.
+   * @param requirements    The subsystems to require.
+   */
+  SwerveControllerCommand(
+      frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+      frc::SwerveDriveKinematics<NumModules> kinematics,
+      frc2::PIDController xController, frc2::PIDController yController,
+      frc::ProfiledPIDController<units::radians> thetaController,
+      std::function<void(std::array<frc::SwerveModuleState, NumModules>)>
+          output,
+      std::initializer_list<Subsystem*> requirements);
+
+  /**
+   * Constructs a new SwerveControllerCommand that when executed will follow the
+   * provided trajectory. This command will not return output voltages but
+   * rather raw module states from the position controllers which need to be put
+   * into a velocity PID.
+   *
+   * <p>Note: The controllers 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.
+   *
+   * <p>Note 2: The rotation controller will calculate the rotation based on the
+   * final pose in the trajectory, not the poses at each time step.
+   *
+   * @param trajectory      The trajectory to follow.
+   * @param pose            A function that supplies the robot pose,
+   *                        provided by the odometry class.
+   * @param kinematics      The kinematics for the robot drivetrain.
+   * @param xController     The Trajectory Tracker PID controller
+   *                        for the robot's x position.
+   * @param yController     The Trajectory Tracker PID controller
+   *                        for the robot's y position.
+   * @param thetaController The Trajectory Tracker PID controller
+   *                        for angle for the robot.
+   * @param output          The raw output module states from the
+   *                        position controllers.
+   * @param requirements    The subsystems to require.
+   */
+  SwerveControllerCommand(
+      frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+      frc::SwerveDriveKinematics<NumModules> kinematics,
+      frc2::PIDController xController, frc2::PIDController yController,
+      frc::ProfiledPIDController<units::radians> thetaController,
+      std::function<void(std::array<frc::SwerveModuleState, NumModules>)>
+          output,
+      wpi::ArrayRef<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::SwerveDriveKinematics<NumModules> m_kinematics;
+  std::unique_ptr<frc2::PIDController> m_xController;
+  std::unique_ptr<frc2::PIDController> m_yController;
+  std::unique_ptr<frc::ProfiledPIDController<units::radians>> m_thetaController;
+  std::function<void(std::array<frc::SwerveModuleState, NumModules>)>
+      m_outputStates;
+
+  frc2::Timer m_timer;
+  units::second_t m_prevTime;
+  frc::Pose2d m_finalPose;
+};
+}  // namespace frc2
+
+#include "SwerveControllerCommand.inc"
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc b/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc
new file mode 100644
index 0000000..42d726d
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc
@@ -0,0 +1,109 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 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>
+
+namespace frc2 {
+
+template <size_t NumModules>
+SwerveControllerCommand<NumModules>::SwerveControllerCommand(
+    frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+    frc::SwerveDriveKinematics<NumModules> kinematics,
+    frc2::PIDController xController, frc2::PIDController yController,
+    frc::ProfiledPIDController<units::radians> thetaController,
+    std::function<void(std::array<frc::SwerveModuleState, NumModules>)> output,
+    std::initializer_list<Subsystem*> requirements)
+    : m_trajectory(trajectory),
+      m_pose(pose),
+      m_kinematics(kinematics),
+      m_xController(std::make_unique<frc2::PIDController>(xController)),
+      m_yController(std::make_unique<frc2::PIDController>(yController)),
+      m_thetaController(
+          std::make_unique<frc::ProfiledPIDController<units::radians>>(
+              thetaController)),
+      m_outputStates(output) {
+  this->AddRequirements(requirements);
+}
+
+template <size_t NumModules>
+SwerveControllerCommand<NumModules>::SwerveControllerCommand(
+    frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+    frc::SwerveDriveKinematics<NumModules> kinematics,
+    frc2::PIDController xController, frc2::PIDController yController,
+    frc::ProfiledPIDController<units::radians> thetaController,
+    std::function<void(std::array<frc::SwerveModuleState, NumModules>)> output,
+    wpi::ArrayRef<Subsystem*> requirements)
+    : m_trajectory(trajectory),
+      m_pose(pose),
+      m_kinematics(kinematics),
+      m_xController(std::make_unique<frc2::PIDController>(xController)),
+      m_yController(std::make_unique<frc2::PIDController>(yController)),
+      m_thetaController(
+          std::make_unique<frc::ProfiledPIDController<units::radians>>(
+              thetaController)),
+      m_outputStates(output) {
+  this->AddRequirements(requirements);
+}
+
+template <size_t NumModules>
+void SwerveControllerCommand<NumModules>::Initialize() {
+  m_finalPose = m_trajectory.Sample(m_trajectory.TotalTime()).pose;
+
+  m_timer.Reset();
+  m_timer.Start();
+}
+
+template <size_t NumModules>
+void SwerveControllerCommand<NumModules>::Execute() {
+  auto curTime = units::second_t(m_timer.Get());
+
+  auto m_desiredState = m_trajectory.Sample(curTime);
+  auto m_desiredPose = m_desiredState.pose;
+
+  auto m_poseError = m_desiredPose.RelativeTo(m_pose());
+
+  auto targetXVel = units::meters_per_second_t(m_xController->Calculate(
+      (m_pose().Translation().X().template to<double>()),
+      (m_desiredPose.Translation().X().template to<double>())));
+  auto targetYVel = units::meters_per_second_t(m_yController->Calculate(
+      (m_pose().Translation().Y().template to<double>()),
+      (m_desiredPose.Translation().Y().template to<double>())));
+
+  // Profiled PID Controller only takes meters as setpoint and measurement
+  // The robot will go to the desired rotation of the final pose in the
+  // trajectory, not following the poses at individual states.
+  auto targetAngularVel =
+      units::radians_per_second_t(m_thetaController->Calculate(
+          m_pose().Rotation().Radians(), m_finalPose.Rotation().Radians()));
+
+  auto vRef = m_desiredState.velocity;
+
+  targetXVel += vRef * m_poseError.Rotation().Cos();
+  targetYVel += vRef * m_poseError.Rotation().Sin();
+
+  auto targetChassisSpeeds =
+      frc::ChassisSpeeds{targetXVel, targetYVel, targetAngularVel};
+
+  auto targetModuleStates =
+      m_kinematics.ToSwerveModuleStates(targetChassisSpeeds);
+
+  m_outputStates(targetModuleStates);
+}
+
+template <size_t NumModules>
+void SwerveControllerCommand<NumModules>::End(bool interrupted) {
+  m_timer.Stop();
+}
+
+template <size_t NumModules>
+bool SwerveControllerCommand<NumModules>::IsFinished() {
+  return m_timer.HasPeriodPassed(m_trajectory.TotalTime());
+}
+
+}  // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h
new file mode 100644
index 0000000..a83dc39
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h
@@ -0,0 +1,85 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 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 <functional>
+#include <initializer_list>
+
+#include <frc/trajectory/TrapezoidProfile.h>
+#include <wpi/ArrayRef.h>
+
+#include "frc2/Timer.h"
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.h"
+
+namespace frc2 {
+/**
+ * A command that runs a TrapezoidProfile.  Useful for smoothly controlling
+ * mechanism motion.
+ *
+ * @see TrapezoidProfile
+ */
+template <class Distance>
+class TrapezoidProfileCommand
+    : public CommandHelper<CommandBase, TrapezoidProfileCommand<Distance>> {
+  using Distance_t = units::unit_t<Distance>;
+  using Velocity =
+      units::compound_unit<Distance, units::inverse<units::seconds>>;
+  using Velocity_t = units::unit_t<Velocity>;
+  using State = typename frc::TrapezoidProfile<Distance>::State;
+
+ 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<Distance> profile,
+                          std::function<void(State)> output,
+                          std::initializer_list<Subsystem*> requirements)
+      : m_profile(profile), m_output(output) {
+    this->AddRequirements(requirements);
+  }
+
+  /**
+   * 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<Distance> profile,
+                          std::function<void(State)> output,
+                          wpi::ArrayRef<Subsystem*> requirements = {})
+      : m_profile(profile), m_output(output) {
+    this->AddRequirements(requirements);
+  }
+
+  void Initialize() override {
+    m_timer.Reset();
+    m_timer.Start();
+  }
+
+  void Execute() override { m_output(m_profile.Calculate(m_timer.Get())); }
+
+  void End(bool interrupted) override { m_timer.Stop(); }
+
+  bool IsFinished() override {
+    return m_timer.HasPeriodPassed(m_profile.TotalTime());
+  }
+
+ private:
+  frc::TrapezoidProfile<Distance> m_profile;
+  std::function<void(State)> m_output;
+
+  Timer m_timer;
+};
+
+}  // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileSubsystem.h b/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileSubsystem.h
new file mode 100644
index 0000000..c5d7e35
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileSubsystem.h
@@ -0,0 +1,97 @@
+/*----------------------------------------------------------------------------*/
+/* 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/trajectory/TrapezoidProfile.h>
+#include <units/units.h>
+
+#include "frc2/command/SubsystemBase.h"
+
+namespace frc2 {
+/**
+ * A subsystem that generates and runs trapezoidal motion profiles
+ * automatically.  The user specifies how to use the current state of the motion
+ * profile by overriding the `UseState` method.
+ */
+template <class Distance>
+class TrapezoidProfileSubsystem : public SubsystemBase {
+  using Distance_t = units::unit_t<Distance>;
+  using Velocity =
+      units::compound_unit<Distance, units::inverse<units::seconds>>;
+  using Velocity_t = units::unit_t<Velocity>;
+  using State = typename frc::TrapezoidProfile<Distance>::State;
+  using Constraints = typename frc::TrapezoidProfile<Distance>::Constraints;
+
+ public:
+  /**
+   * Creates a new TrapezoidProfileSubsystem.
+   *
+   * @param constraints     The constraints (maximum velocity and acceleration)
+   * for the profiles.
+   * @param initialPosition The initial position of the controller mechanism
+   * when the subsystem is constructed.
+   * @param period          The period of the main robot loop, in seconds.
+   */
+  explicit TrapezoidProfileSubsystem(Constraints constraints,
+                                     Distance_t initialPosition = Distance_t{0},
+                                     units::second_t period = 20_ms)
+      : m_constraints(constraints),
+        m_state{initialPosition, Velocity_t(0)},
+        m_goal{initialPosition, Velocity_t{0}},
+        m_period(period) {}
+
+  void Periodic() override {
+    auto profile =
+        frc::TrapezoidProfile<Distance>(m_constraints, m_goal, m_state);
+    m_state = profile.Calculate(m_period);
+    if (m_enabled) {
+      UseState(m_state);
+    }
+  }
+
+  /**
+   * Sets the goal state for the subsystem.
+   *
+   * @param goal The goal state for the subsystem's motion profile.
+   */
+  void SetGoal(State goal) { m_goal = goal; }
+
+  /**
+   * Sets the goal state for the subsystem.  Goal velocity assumed to be zero.
+   *
+   * @param goal The goal position for the subsystem's motion profile.
+   */
+  void SetGoal(Distance_t goal) { m_goal = State{goal, Velocity_t(0)}; }
+
+ protected:
+  /**
+   * Users should override this to consume the current state of the motion
+   * profile.
+   *
+   * @param state The current state of the motion profile.
+   */
+  virtual void UseState(State state) = 0;
+
+  /**
+   * Enable the TrapezoidProfileSubsystem's output.
+   */
+  void Enable() { m_enabled = true; }
+
+  /**
+   * Disable the TrapezoidProfileSubsystem's output.
+   */
+  void Disable() { m_enabled = false; }
+
+ private:
+  Constraints m_constraints;
+  State m_state;
+  State m_goal;
+  units::second_t m_period;
+  bool m_enabled{false};
+};
+}  // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/WaitCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/WaitCommand.h
new file mode 100644
index 0000000..ab97958
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/WaitCommand.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 <units/units.h>
+
+#include "frc2/Timer.h"
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.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
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/WaitUntilCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/WaitUntilCommand.h
new file mode 100644
index 0000000..50b4855
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/WaitUntilCommand.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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <functional>
+
+#include <units/units.h>
+
+#include "frc2/command/CommandBase.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(units::second_t 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
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/button/Button.h b/wpilibNewCommands/src/main/native/include/frc2/command/button/Button.h
new file mode 100644
index 0000000..af861d4
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/button/Button.h
@@ -0,0 +1,245 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 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 <functional>
+#include <initializer_list>
+#include <utility>
+
+#include <wpi/ArrayRef.h>
+
+#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.
+   * @param requirements the required subsystems.
+   */
+  Button WhenPressed(std::function<void()> toRun,
+                     std::initializer_list<Subsystem*> requirements);
+
+  /**
+   * Binds a runnable to execute when the button is pressed.
+   *
+   * @param toRun the runnable to execute.
+   * @param requirements the required subsystems.
+   */
+  Button WhenPressed(std::function<void()> toRun,
+                     wpi::ArrayRef<Subsystem*> requirements = {});
+
+  /**
+   * 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.
+   * @param requirements the required subsystems.
+   */
+  Button WhileHeld(std::function<void()> toRun,
+                   std::initializer_list<Subsystem*> requirements);
+
+  /**
+   * Binds a runnable to execute repeatedly while the button is pressed.
+   *
+   * @param toRun the runnable to execute.
+   * @param requirements the required subsystems.
+   */
+  Button WhileHeld(std::function<void()> toRun,
+                   wpi::ArrayRef<Subsystem*> requirements = {});
+
+  /**
+   * 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.
+   * @param requirements the required subsystems.
+   */
+  Button WhenReleased(std::function<void()> toRun,
+                      std::initializer_list<Subsystem*> requirements);
+
+  /**
+   * Binds a runnable to execute when the button is released.
+   *
+   * @param toRun the runnable to execute.
+   * @param requirements the required subsystems.
+   */
+  Button WhenReleased(std::function<void()> toRun,
+                      wpi::ArrayRef<Subsystem*> requirements = {});
+
+  /**
+   * 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
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/button/JoystickButton.h b/wpilibNewCommands/src/main/native/include/frc2/command/button/JoystickButton.h
new file mode 100644
index 0000000..a23738b
--- /dev/null
+++ b/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
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/button/POVButton.h b/wpilibNewCommands/src/main/native/include/frc2/command/button/POVButton.h
new file mode 100644
index 0000000..758cab2
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/button/POVButton.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 <frc/GenericHID.h>
+
+#include "Button.h"
+
+namespace frc2 {
+/**
+ * A class used to bind command scheduling to joystick POV presses.  Can be
+ * composed with other buttons with the operators in Trigger.
+ *
+ * @see Trigger
+ */
+class POVButton : public Button {
+ public:
+  /**
+   * Creates a POVButton that commands can be bound to.
+   *
+   * @param joystick The joystick on which the button is located.
+   * @param angle The angle of the POV corresponding to a button press.
+   * @param povNumber The number of the POV on the joystick.
+   */
+  POVButton(frc::GenericHID* joystick, int angle, int povNumber = 0)
+      : m_joystick{joystick}, m_angle{angle}, m_povNumber{povNumber} {}
+
+  bool Get() const override {
+    return m_joystick->GetPOV(m_povNumber) == m_angle;
+  }
+
+ private:
+  frc::GenericHID* m_joystick;
+  int m_angle;
+  int m_povNumber;
+};
+}  // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h b/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h
new file mode 100644
index 0000000..7df6b4e
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h
@@ -0,0 +1,361 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 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 <functional>
+#include <initializer_list>
+#include <memory>
+#include <utility>
+
+#include <wpi/ArrayRef.h>
+
+#include "frc2/command/Command.h"
+#include "frc2/command/CommandScheduler.h"
+
+namespace frc2 {
+class Command;
+/**
+ * A class used to bind command scheduling to events.  The
+ * Trigger class is a base for all command-event-binding classes, and so the
+ * methods are named fairly abstractly; for purpose-specific wrappers, see
+ * Button.
+ *
+ * @see Button
+ */
+class Trigger {
+ public:
+  /**
+   * Create a new trigger that is active when the given condition is true.
+   *
+   * @param isActive Whether the trigger is active.
+   */
+  explicit Trigger(std::function<bool()> isActive)
+      : m_isActive{std::move(isActive)} {}
+
+  /**
+   * Create a new trigger that is never active (default constructor) - activity
+   *  can be further determined by subclass code.
+   */
+  Trigger() {
+    m_isActive = [] { return false; };
+  }
+
+  Trigger(const Trigger& other);
+
+  /**
+   * Returns whether the trigger is active.  Can be overridden by a subclass.
+   *
+   * @return Whether the trigger is active.
+   */
+  virtual bool Get() const { return m_isActive(); }
+
+  /**
+   * Binds a command to start when the trigger becomes active.  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.
+   */
+  Trigger WhenActive(Command* command, bool interruptible = true);
+
+  /**
+   * Binds a command to start when the trigger becomes active.  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>>>>
+  Trigger WhenActive(T&& command, bool interruptible = true) {
+    CommandScheduler::GetInstance().AddButton(
+        [pressedLast = Get(), *this,
+         command = std::make_unique<std::remove_reference_t<T>>(
+             std::forward<T>(command)),
+         interruptible]() mutable {
+          bool pressed = Get();
+
+          if (!pressedLast && pressed) {
+            command->Schedule(interruptible);
+          }
+
+          pressedLast = pressed;
+        });
+
+    return *this;
+  }
+
+  /**
+   * Binds a runnable to execute when the trigger becomes active.
+   *
+   * @param toRun the runnable to execute.
+   * @paaram requirements the required subsystems.
+   */
+  Trigger WhenActive(std::function<void()> toRun,
+                     std::initializer_list<Subsystem*> requirements);
+
+  /**
+   * Binds a runnable to execute when the trigger becomes active.
+   *
+   * @param toRun the runnable to execute.
+   * @paaram requirements the required subsystems.
+   */
+  Trigger WhenActive(std::function<void()> toRun,
+                     wpi::ArrayRef<Subsystem*> requirements = {});
+
+  /**
+   * Binds a command to be started repeatedly while the trigger is active, and
+   * cancelled when it becomes inactive.  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.
+   */
+  Trigger WhileActiveContinous(Command* command, bool interruptible = true);
+
+  /**
+   * Binds a command to be started repeatedly while the trigger is active, and
+   * cancelled when it becomes inactive.  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>>>>
+  Trigger WhileActiveContinous(T&& command, bool interruptible = true) {
+    CommandScheduler::GetInstance().AddButton(
+        [pressedLast = Get(), *this,
+         command = std::make_unique<std::remove_reference_t<T>>(
+             std::forward<T>(command)),
+         interruptible]() mutable {
+          bool pressed = Get();
+
+          if (pressed) {
+            command->Schedule(interruptible);
+          } else if (pressedLast && !pressed) {
+            command->Cancel();
+          }
+
+          pressedLast = pressed;
+        });
+    return *this;
+  }
+
+  /**
+   * Binds a runnable to execute repeatedly while the trigger is active.
+   *
+   * @param toRun the runnable to execute.
+   * @param requirements the required subsystems.
+   */
+  Trigger WhileActiveContinous(std::function<void()> toRun,
+                               std::initializer_list<Subsystem*> requirements);
+
+  /**
+   * Binds a runnable to execute repeatedly while the trigger is active.
+   *
+   * @param toRun the runnable to execute.
+   * @param requirements the required subsystems.
+   */
+  Trigger WhileActiveContinous(std::function<void()> toRun,
+                               wpi::ArrayRef<Subsystem*> requirements = {});
+
+  /**
+   * Binds a command to be started when the trigger becomes active, and
+   * cancelled when it becomes inactive.  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.
+   */
+  Trigger WhileActiveOnce(Command* command, bool interruptible = true);
+
+  /**
+   * Binds a command to be started when the trigger becomes active, and
+   * cancelled when it becomes inactive.  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>>>>
+  Trigger WhileActiveOnce(T&& command, bool interruptible = true) {
+    CommandScheduler::GetInstance().AddButton(
+        [pressedLast = Get(), *this,
+         command = std::make_unique<std::remove_reference_t<T>>(
+             std::forward<T>(command)),
+         interruptible]() mutable {
+          bool pressed = Get();
+
+          if (!pressedLast && pressed) {
+            command->Schedule(interruptible);
+          } else if (pressedLast && !pressed) {
+            command->Cancel();
+          }
+
+          pressedLast = pressed;
+        });
+    return *this;
+  }
+
+  /**
+   * Binds a command to start when the trigger becomes inactive.  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.
+   */
+  Trigger WhenInactive(Command* command, bool interruptible = true);
+
+  /**
+   * Binds a command to start when the trigger becomes inactive.  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>>>>
+  Trigger WhenInactive(T&& command, bool interruptible = true) {
+    CommandScheduler::GetInstance().AddButton(
+        [pressedLast = Get(), *this,
+         command = std::make_unique<std::remove_reference_t<T>>(
+             std::forward<T>(command)),
+         interruptible]() mutable {
+          bool pressed = Get();
+
+          if (pressedLast && !pressed) {
+            command->Schedule(interruptible);
+          }
+
+          pressedLast = pressed;
+        });
+    return *this;
+  }
+
+  /**
+   * Binds a runnable to execute when the trigger becomes inactive.
+   *
+   * @param toRun the runnable to execute.
+   * @param requirements the required subsystems.
+   */
+  Trigger WhenInactive(std::function<void()> toRun,
+                       std::initializer_list<Subsystem*> requirements);
+
+  /**
+   * Binds a runnable to execute when the trigger becomes inactive.
+   *
+   * @param toRun the runnable to execute.
+   * @param requirements the required subsystems.
+   */
+  Trigger WhenInactive(std::function<void()> toRun,
+                       wpi::ArrayRef<Subsystem*> requirements = {});
+
+  /**
+   * Binds a command to start when the trigger becomes active, and be cancelled
+   * when it again becomes active.  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.
+   */
+  Trigger ToggleWhenActive(Command* command, bool interruptible = true);
+
+  /**
+   * Binds a command to start when the trigger becomes active, and be cancelled
+   * when it again becomes active.  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>>>>
+  Trigger ToggleWhenActive(T&& command, bool interruptible = true) {
+    CommandScheduler::GetInstance().AddButton(
+        [pressedLast = Get(), *this,
+         command = std::make_unique<std::remove_reference_t<T>>(
+             std::forward<T>(command)),
+         interruptible]() mutable {
+          bool pressed = Get();
+
+          if (!pressedLast && pressed) {
+            if (command->IsScheduled()) {
+              command->Cancel();
+            } else {
+              command->Schedule(interruptible);
+            }
+          }
+
+          pressedLast = pressed;
+        });
+    return *this;
+  }
+
+  /**
+   * Binds a command to be cancelled when the trigger becomes active.  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 trigger, for chained calls.
+   */
+  Trigger CancelWhenActive(Command* command);
+
+  /**
+   * Composes two triggers with logical AND.
+   *
+   * @return A trigger which is active when both component triggers are active.
+   */
+  Trigger operator&&(Trigger rhs) {
+    return Trigger([*this, rhs] { return Get() && rhs.Get(); });
+  }
+
+  /**
+   * Composes two triggers with logical OR.
+   *
+   * @return A trigger which is active when either component trigger is active.
+   */
+  Trigger operator||(Trigger rhs) {
+    return Trigger([*this, rhs] { return Get() || rhs.Get(); });
+  }
+
+  /**
+   * Composes a trigger with logical NOT.
+   *
+   * @return A trigger which is active when the component trigger is inactive,
+   * and vice-versa.
+   */
+  Trigger operator!() {
+    return Trigger([*this] { return !Get(); });
+  }
+
+ private:
+  std::function<bool()> m_isActive;
+};
+}  // namespace frc2
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/MockHardwareExtension.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/MockHardwareExtension.java
new file mode 100644
index 0000000..4f9abf0
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/MockHardwareExtension.java
@@ -0,0 +1,40 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-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;
+
+import org.junit.jupiter.api.extension.BeforeAllCallback;
+import org.junit.jupiter.api.extension.ExtensionContext;
+import org.junit.jupiter.api.extension.ExtensionContext.Namespace;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.sim.DriverStationSim;
+
+public final class MockHardwareExtension implements BeforeAllCallback {
+  private static ExtensionContext getRoot(ExtensionContext context) {
+    return context.getParent().map(MockHardwareExtension::getRoot).orElse(context);
+  }
+
+  @Override
+  public void beforeAll(ExtensionContext context) {
+    getRoot(context).getStore(Namespace.GLOBAL).getOrComputeIfAbsent("HAL Initalized", key -> {
+      initializeHardware();
+      return true;
+    }, Boolean.class);
+  }
+
+  private void initializeHardware() {
+    HAL.initialize(500, 0);
+    DriverStationSim dsSim = new DriverStationSim();
+    dsSim.setDsAttached(true);
+    dsSim.setAutonomous(false);
+    dsSim.setEnabled(true);
+    dsSim.setTest(true);
+
+
+  }
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ButtonTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ButtonTest.java
new file mode 100644
index 0000000..64354c3
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ButtonTest.java
@@ -0,0 +1,179 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj2.command.button.InternalButton;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.mockito.Mockito.never;
+import static org.mockito.Mockito.times;
+import static org.mockito.Mockito.verify;
+import static org.mockito.Mockito.when;
+
+
+class ButtonTest extends CommandTestBase {
+  @Test
+  void whenPressedTest() {
+    CommandScheduler scheduler = CommandScheduler.getInstance();
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+
+    InternalButton button = new InternalButton();
+    button.setPressed(false);
+    button.whenPressed(command1);
+    scheduler.run();
+    verify(command1, never()).schedule(true);
+    button.setPressed(true);
+    scheduler.run();
+    scheduler.run();
+    verify(command1).schedule(true);
+  }
+
+  @Test
+  void whenReleasedTest() {
+    CommandScheduler scheduler = CommandScheduler.getInstance();
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+
+    InternalButton button = new InternalButton();
+    button.setPressed(true);
+    button.whenReleased(command1);
+    scheduler.run();
+    verify(command1, never()).schedule(true);
+    button.setPressed(false);
+    scheduler.run();
+    scheduler.run();
+    verify(command1).schedule(true);
+  }
+
+  @Test
+  void whileHeldTest() {
+    CommandScheduler scheduler = CommandScheduler.getInstance();
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+
+    InternalButton button = new InternalButton();
+    button.setPressed(false);
+    button.whileHeld(command1);
+    scheduler.run();
+    verify(command1, never()).schedule(true);
+    button.setPressed(true);
+    scheduler.run();
+    scheduler.run();
+    verify(command1, times(2)).schedule(true);
+    button.setPressed(false);
+    scheduler.run();
+    verify(command1).cancel();
+  }
+
+  @Test
+  void whenHeldTest() {
+    CommandScheduler scheduler = CommandScheduler.getInstance();
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+
+    InternalButton button = new InternalButton();
+    button.setPressed(false);
+    button.whenHeld(command1);
+    scheduler.run();
+    verify(command1, never()).schedule(true);
+    button.setPressed(true);
+    scheduler.run();
+    scheduler.run();
+    verify(command1).schedule(true);
+    button.setPressed(false);
+    scheduler.run();
+    verify(command1).cancel();
+  }
+
+  @Test
+  void toggleWhenPressedTest() {
+    CommandScheduler scheduler = CommandScheduler.getInstance();
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+
+    InternalButton button = new InternalButton();
+    button.setPressed(false);
+    button.toggleWhenPressed(command1);
+    scheduler.run();
+    verify(command1, never()).schedule(true);
+    button.setPressed(true);
+    scheduler.run();
+    when(command1.isScheduled()).thenReturn(true);
+    scheduler.run();
+    verify(command1).schedule(true);
+    button.setPressed(false);
+    scheduler.run();
+    verify(command1, never()).cancel();
+    button.setPressed(true);
+    scheduler.run();
+    verify(command1).cancel();
+  }
+
+  @Test
+  void cancelWhenPressedTest() {
+    CommandScheduler scheduler = CommandScheduler.getInstance();
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+
+    InternalButton button = new InternalButton();
+    button.setPressed(false);
+    button.cancelWhenPressed(command1);
+    scheduler.run();
+    verify(command1, never()).cancel();
+    button.setPressed(true);
+    scheduler.run();
+    scheduler.run();
+    verify(command1).cancel();
+  }
+
+  @Test
+  void runnableBindingTest() {
+
+    InternalButton buttonWhenPressed = new InternalButton();
+    InternalButton buttonWhileHeld = new InternalButton();
+    InternalButton buttonWhenReleased = new InternalButton();
+
+    buttonWhenPressed.setPressed(false);
+    buttonWhileHeld.setPressed(true);
+    buttonWhenReleased.setPressed(true);
+
+    Counter counter = new Counter();
+
+    buttonWhenPressed.whenPressed(counter::increment);
+    buttonWhileHeld.whileHeld(counter::increment);
+    buttonWhenReleased.whenReleased(counter::increment);
+
+    CommandScheduler scheduler = CommandScheduler.getInstance();
+
+    scheduler.run();
+    buttonWhenPressed.setPressed(true);
+    buttonWhenReleased.setPressed(false);
+    scheduler.run();
+
+    assertEquals(counter.m_counter, 4);
+  }
+
+  @Test
+  void buttonCompositionTest() {
+    InternalButton button1 = new InternalButton();
+    InternalButton button2 = new InternalButton();
+
+    button1.setPressed(true);
+    button2.setPressed(false);
+
+    assertFalse(button1.and(button2).get());
+    assertTrue(button1.or(button2).get());
+    assertFalse(button1.negate().get());
+    assertTrue(button1.and(button2.negate()).get());
+  }
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandDecoratorTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandDecoratorTest.java
new file mode 100644
index 0000000..23f63c7
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandDecoratorTest.java
@@ -0,0 +1,182 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.Timer;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class CommandDecoratorTest extends CommandTestBase {
+  @Test
+  void withTimeoutTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    Command command1 = new WaitCommand(10);
+
+    Command timeout = command1.withTimeout(2);
+
+    scheduler.schedule(timeout);
+    scheduler.run();
+
+    assertFalse(scheduler.isScheduled(command1));
+    assertTrue(scheduler.isScheduled(timeout));
+
+    Timer.delay(3);
+    scheduler.run();
+
+    assertFalse(scheduler.isScheduled(timeout));
+  }
+
+  @Test
+  void withInterruptTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    ConditionHolder condition = new ConditionHolder();
+
+    Command command = new WaitCommand(10).withInterrupt(condition::getCondition);
+
+    scheduler.schedule(command);
+    scheduler.run();
+    assertTrue(scheduler.isScheduled(command));
+    condition.setCondition(true);
+    scheduler.run();
+    assertFalse(scheduler.isScheduled(command));
+  }
+
+  @Test
+  void beforeStartingTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    ConditionHolder condition = new ConditionHolder();
+    condition.setCondition(false);
+
+    Command command = new InstantCommand();
+
+    scheduler.schedule(command.beforeStarting(() -> condition.setCondition(true)));
+
+    assertTrue(condition.getCondition());
+  }
+
+  @Test
+  void andThenLambdaTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    ConditionHolder condition = new ConditionHolder();
+    condition.setCondition(false);
+
+    Command command = new InstantCommand();
+
+    scheduler.schedule(command.andThen(() -> condition.setCondition(true)));
+
+    assertFalse(condition.getCondition());
+
+    scheduler.run();
+
+    assertTrue(condition.getCondition());
+  }
+
+  @Test
+  void andThenTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    ConditionHolder condition = new ConditionHolder();
+    condition.setCondition(false);
+
+    Command command1 = new InstantCommand();
+    Command command2 = new InstantCommand(() -> condition.setCondition(true));
+
+    scheduler.schedule(command1.andThen(command2));
+
+    assertFalse(condition.getCondition());
+
+    scheduler.run();
+
+    assertTrue(condition.getCondition());
+  }
+
+  @Test
+  void deadlineWithTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    ConditionHolder condition = new ConditionHolder();
+    condition.setCondition(false);
+
+    Command dictator = new WaitUntilCommand(condition::getCondition);
+    Command endsBefore = new InstantCommand();
+    Command endsAfter = new WaitUntilCommand(() -> false);
+
+    Command group = dictator.deadlineWith(endsBefore, endsAfter);
+
+    scheduler.schedule(group);
+    scheduler.run();
+
+    assertTrue(scheduler.isScheduled(group));
+
+    condition.setCondition(true);
+    scheduler.run();
+
+    assertFalse(scheduler.isScheduled(group));
+  }
+
+  @Test
+  void alongWithTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    ConditionHolder condition = new ConditionHolder();
+    condition.setCondition(false);
+
+    Command command1 = new WaitUntilCommand(condition::getCondition);
+    Command command2 = new InstantCommand();
+
+    Command group = command1.alongWith(command2);
+
+    scheduler.schedule(group);
+    scheduler.run();
+
+    assertTrue(scheduler.isScheduled(group));
+
+    condition.setCondition(true);
+    scheduler.run();
+
+    assertFalse(scheduler.isScheduled(group));
+  }
+
+  @Test
+  void raceWithTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    Command command1 = new WaitUntilCommand(() -> false);
+    Command command2 = new InstantCommand();
+
+    Command group = command1.raceWith(command2);
+
+    scheduler.schedule(group);
+    scheduler.run();
+
+    assertFalse(scheduler.isScheduled(group));
+  }
+
+  @Test
+  void perpetuallyTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    Command command = new InstantCommand();
+
+    Command perpetual = command.perpetually();
+
+    scheduler.schedule(perpetual);
+    scheduler.run();
+    scheduler.run();
+    scheduler.run();
+
+    assertTrue(scheduler.isScheduled(perpetual));
+  }
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandGroupErrorTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandGroupErrorTest.java
new file mode 100644
index 0000000..e453d94
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandGroupErrorTest.java
@@ -0,0 +1,55 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+
+class CommandGroupErrorTest extends CommandTestBase {
+  @Test
+  void commandInMultipleGroupsTest() {
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true);
+    Command command2 = command2Holder.getMock();
+
+    @SuppressWarnings("PMD.UnusedLocalVariable")
+    Command group = new ParallelCommandGroup(command1, command2);
+    assertThrows(IllegalArgumentException.class,
+        () -> new ParallelCommandGroup(command1, command2));
+  }
+
+  @Test
+  void commandInGroupExternallyScheduledTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true);
+    Command command2 = command2Holder.getMock();
+
+    @SuppressWarnings("PMD.UnusedLocalVariable")
+    Command group = new ParallelCommandGroup(command1, command2);
+
+    assertThrows(IllegalArgumentException.class,
+        () -> scheduler.schedule(command1));
+  }
+
+  @Test
+  void redecoratedCommandErrorTest() {
+    Command command = new InstantCommand();
+
+    assertDoesNotThrow(() -> command.withTimeout(10).withInterrupt(() -> false));
+    assertThrows(IllegalArgumentException.class, () -> command.withTimeout(10));
+    CommandGroupBase.clearGroupedCommand(command);
+    assertDoesNotThrow(() -> command.withTimeout(10));
+  }
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandRequirementsTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandRequirementsTest.java
new file mode 100644
index 0000000..10d3ee0
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandRequirementsTest.java
@@ -0,0 +1,79 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.mockito.Mockito.verify;
+
+@SuppressWarnings("PMD.TooManyMethods")
+class CommandRequirementsTest extends CommandTestBase {
+  @Test
+  void requirementInterruptTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    Subsystem requirement = new TestSubsystem();
+
+    MockCommandHolder interruptedHolder = new MockCommandHolder(true, requirement);
+    Command interrupted = interruptedHolder.getMock();
+    MockCommandHolder interrupterHolder = new MockCommandHolder(true, requirement);
+    Command interrupter = interrupterHolder.getMock();
+
+    scheduler.schedule(interrupted);
+    scheduler.run();
+    scheduler.schedule(interrupter);
+    scheduler.run();
+
+    verify(interrupted).initialize();
+    verify(interrupted).execute();
+    verify(interrupted).end(true);
+
+    verify(interrupter).initialize();
+    verify(interrupter).execute();
+
+    assertFalse(scheduler.isScheduled(interrupted));
+    assertTrue(scheduler.isScheduled(interrupter));
+  }
+
+  @Test
+  void requirementUninterruptibleTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    Subsystem requirement = new TestSubsystem();
+
+    MockCommandHolder interruptedHolder = new MockCommandHolder(true, requirement);
+    Command notInterrupted = interruptedHolder.getMock();
+    MockCommandHolder interrupterHolder = new MockCommandHolder(true, requirement);
+    Command interrupter = interrupterHolder.getMock();
+
+    scheduler.schedule(false, notInterrupted);
+    scheduler.schedule(interrupter);
+
+    assertTrue(scheduler.isScheduled(notInterrupted));
+    assertFalse(scheduler.isScheduled(interrupter));
+  }
+
+  @Test
+  void defaultCommandRequirementErrorTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    Subsystem system = new TestSubsystem();
+
+    Command missingRequirement = new WaitUntilCommand(() -> false);
+    Command ends = new InstantCommand(() -> {
+    }, system);
+
+    assertThrows(IllegalArgumentException.class,
+        () -> scheduler.setDefaultCommand(system, missingRequirement));
+    assertThrows(IllegalArgumentException.class,
+        () -> scheduler.setDefaultCommand(system, ends));
+  }
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandScheduleTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandScheduleTest.java
new file mode 100644
index 0000000..aee30db
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandScheduleTest.java
@@ -0,0 +1,122 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.mockito.Mockito.never;
+import static org.mockito.Mockito.times;
+import static org.mockito.Mockito.verify;
+
+class CommandScheduleTest extends CommandTestBase {
+  @Test
+  void instantScheduleTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder holder = new MockCommandHolder(true);
+    holder.setFinished(true);
+    Command mockCommand = holder.getMock();
+
+    scheduler.schedule(mockCommand);
+    assertTrue(scheduler.isScheduled(mockCommand));
+    verify(mockCommand).initialize();
+
+    scheduler.run();
+
+    verify(mockCommand).execute();
+    verify(mockCommand).end(false);
+
+    assertFalse(scheduler.isScheduled(mockCommand));
+  }
+
+  @Test
+  void singleIterationScheduleTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder holder = new MockCommandHolder(true);
+    Command mockCommand = holder.getMock();
+
+    scheduler.schedule(mockCommand);
+
+    assertTrue(scheduler.isScheduled(mockCommand));
+
+    scheduler.run();
+    holder.setFinished(true);
+    scheduler.run();
+
+    verify(mockCommand).initialize();
+    verify(mockCommand, times(2)).execute();
+    verify(mockCommand).end(false);
+
+    assertFalse(scheduler.isScheduled(mockCommand));
+  }
+
+  @Test
+  void multiScheduleTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true);
+    Command command2 = command2Holder.getMock();
+    MockCommandHolder command3Holder = new MockCommandHolder(true);
+    Command command3 = command3Holder.getMock();
+
+    scheduler.schedule(true, command1, command2, command3);
+    assertTrue(scheduler.isScheduled(command1, command2, command3));
+    scheduler.run();
+    assertTrue(scheduler.isScheduled(command1, command2, command3));
+
+    command1Holder.setFinished(true);
+    scheduler.run();
+    assertTrue(scheduler.isScheduled(command2, command3));
+    assertFalse(scheduler.isScheduled(command1));
+
+    command2Holder.setFinished(true);
+    scheduler.run();
+    assertTrue(scheduler.isScheduled(command3));
+    assertFalse(scheduler.isScheduled(command1, command2));
+
+    command3Holder.setFinished(true);
+    scheduler.run();
+    assertFalse(scheduler.isScheduled(command1, command2, command3));
+  }
+
+  @Test
+  void schedulerCancelTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder holder = new MockCommandHolder(true);
+    Command mockCommand = holder.getMock();
+
+    scheduler.schedule(mockCommand);
+
+    scheduler.run();
+    scheduler.cancel(mockCommand);
+    scheduler.run();
+
+    verify(mockCommand).execute();
+    verify(mockCommand).end(true);
+    verify(mockCommand, never()).end(false);
+
+    assertFalse(scheduler.isScheduled(mockCommand));
+  }
+
+  @Test
+  void notScheduledCancelTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder holder = new MockCommandHolder(true);
+    Command mockCommand = holder.getMock();
+
+    assertDoesNotThrow(() -> scheduler.cancel(mockCommand));
+  }
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandTestBase.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandTestBase.java
new file mode 100644
index 0000000..61aa1a0
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandTestBase.java
@@ -0,0 +1,92 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.Set;
+
+import org.junit.jupiter.api.BeforeEach;
+
+import edu.wpi.first.hal.sim.DriverStationSim;
+import edu.wpi.first.wpilibj.DriverStation;
+
+import static org.mockito.Mockito.mock;
+import static org.mockito.Mockito.when;
+
+/**
+ * Basic setup for all {@link Command tests}."
+ */
+@SuppressWarnings("PMD.AbstractClassWithoutAbstractMethod")
+abstract class CommandTestBase {
+  @BeforeEach
+  void commandSetup() {
+    CommandScheduler.getInstance().cancelAll();
+    CommandScheduler.getInstance().enable();
+    CommandScheduler.getInstance().clearButtons();
+    CommandGroupBase.clearGroupedCommands();
+
+    setDSEnabled(true);
+  }
+
+  void setDSEnabled(boolean enabled) {
+    DriverStationSim sim = new DriverStationSim();
+    sim.setDsAttached(true);
+
+    sim.setEnabled(enabled);
+    sim.notifyNewData();
+    DriverStation.getInstance().isNewControlData();
+    while (DriverStation.getInstance().isEnabled() != enabled) {
+      try {
+        Thread.sleep(1);
+      } catch (InterruptedException exception) {
+        exception.printStackTrace();
+      }
+    }
+  }
+
+  class TestSubsystem extends SubsystemBase {
+  }
+
+  protected class MockCommandHolder {
+    private final Command m_mockCommand = mock(Command.class);
+
+    MockCommandHolder(boolean runWhenDisabled, Subsystem... requirements) {
+      when(m_mockCommand.getRequirements()).thenReturn(Set.of(requirements));
+      when(m_mockCommand.isFinished()).thenReturn(false);
+      when(m_mockCommand.runsWhenDisabled()).thenReturn(runWhenDisabled);
+    }
+
+    Command getMock() {
+      return m_mockCommand;
+    }
+
+    void setFinished(boolean finished) {
+      when(m_mockCommand.isFinished()).thenReturn(finished);
+    }
+
+  }
+
+  protected class Counter {
+    int m_counter;
+
+    void increment() {
+      m_counter++;
+    }
+  }
+
+  protected class ConditionHolder {
+    private boolean m_condition;
+
+    void setCondition(boolean condition) {
+      m_condition = condition;
+    }
+
+    boolean getCondition() {
+      return m_condition;
+    }
+  }
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ConditionalCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ConditionalCommandTest.java
new file mode 100644
index 0000000..0740565
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ConditionalCommandTest.java
@@ -0,0 +1,65 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.mockito.Mockito.never;
+import static org.mockito.Mockito.verify;
+
+class ConditionalCommandTest extends CommandTestBase {
+  @Test
+  void conditionalCommandTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+    command1Holder.setFinished(true);
+    MockCommandHolder command2Holder = new MockCommandHolder(true);
+    Command command2 = command2Holder.getMock();
+
+    ConditionalCommand conditionalCommand = new ConditionalCommand(command1, command2, () -> true);
+
+    scheduler.schedule(conditionalCommand);
+    scheduler.run();
+
+    verify(command1).initialize();
+    verify(command1).execute();
+    verify(command1).end(false);
+
+    verify(command2, never()).initialize();
+    verify(command2, never()).execute();
+    verify(command2, never()).end(false);
+  }
+
+  @Test
+  void conditionalCommandRequirementTest() {
+    Subsystem system1 = new TestSubsystem();
+    Subsystem system2 = new TestSubsystem();
+    Subsystem system3 = new TestSubsystem();
+
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true, system3);
+    Command command2 = command2Holder.getMock();
+
+    ConditionalCommand conditionalCommand = new ConditionalCommand(command1, command2, () -> true);
+
+    scheduler.schedule(conditionalCommand);
+    scheduler.schedule(new InstantCommand(() -> {
+    }, system3));
+
+    assertFalse(scheduler.isScheduled(conditionalCommand));
+
+    verify(command1).end(true);
+    verify(command2, never()).end(true);
+  }
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/DefaultCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/DefaultCommandTest.java
new file mode 100644
index 0000000..033dcc5
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/DefaultCommandTest.java
@@ -0,0 +1,83 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.mockito.Mockito.verify;
+
+class DefaultCommandTest extends CommandTestBase {
+  @Test
+  void defaultCommandScheduleTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    TestSubsystem hasDefaultCommand = new TestSubsystem();
+
+    MockCommandHolder defaultHolder = new MockCommandHolder(true, hasDefaultCommand);
+    Command defaultCommand = defaultHolder.getMock();
+
+    scheduler.setDefaultCommand(hasDefaultCommand, defaultCommand);
+    scheduler.run();
+
+    assertTrue(scheduler.isScheduled(defaultCommand));
+  }
+
+  @Test
+  void defaultCommandInterruptResumeTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    TestSubsystem hasDefaultCommand = new TestSubsystem();
+
+    MockCommandHolder defaultHolder = new MockCommandHolder(true, hasDefaultCommand);
+    Command defaultCommand = defaultHolder.getMock();
+    MockCommandHolder interrupterHolder = new MockCommandHolder(true, hasDefaultCommand);
+    Command interrupter = interrupterHolder.getMock();
+
+    scheduler.setDefaultCommand(hasDefaultCommand, defaultCommand);
+    scheduler.run();
+    scheduler.schedule(interrupter);
+
+    assertFalse(scheduler.isScheduled(defaultCommand));
+    assertTrue(scheduler.isScheduled(interrupter));
+
+    scheduler.cancel(interrupter);
+    scheduler.run();
+
+    assertTrue(scheduler.isScheduled(defaultCommand));
+    assertFalse(scheduler.isScheduled(interrupter));
+  }
+
+  @Test
+  void defaultCommandDisableResumeTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    TestSubsystem hasDefaultCommand = new TestSubsystem();
+
+    MockCommandHolder defaultHolder = new MockCommandHolder(false, hasDefaultCommand);
+    Command defaultCommand = defaultHolder.getMock();
+
+    scheduler.setDefaultCommand(hasDefaultCommand, defaultCommand);
+    scheduler.run();
+
+    assertTrue(scheduler.isScheduled(defaultCommand));
+
+    setDSEnabled(false);
+    scheduler.run();
+
+    assertFalse(scheduler.isScheduled(defaultCommand));
+
+    setDSEnabled(true);
+    scheduler.run();
+
+    assertTrue(scheduler.isScheduled(defaultCommand));
+
+    verify(defaultCommand).end(true);
+  }
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/FunctionalCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/FunctionalCommandTest.java
new file mode 100644
index 0000000..23f508a
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/FunctionalCommandTest.java
@@ -0,0 +1,43 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class FunctionalCommandTest extends CommandTestBase {
+  @Test
+  void functionalCommandScheduleTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    ConditionHolder cond1 = new ConditionHolder();
+    ConditionHolder cond2 = new ConditionHolder();
+    ConditionHolder cond3 = new ConditionHolder();
+    ConditionHolder cond4 = new ConditionHolder();
+
+    FunctionalCommand command =
+        new FunctionalCommand(() -> cond1.setCondition(true), () -> cond2.setCondition(true),
+            interrupted -> cond3.setCondition(true), cond4::getCondition);
+
+    scheduler.schedule(command);
+    scheduler.run();
+
+    assertTrue(scheduler.isScheduled(command));
+
+    cond4.setCondition(true);
+
+    scheduler.run();
+
+    assertFalse(scheduler.isScheduled(command));
+    assertTrue(cond1.getCondition());
+    assertTrue(cond2.getCondition());
+    assertTrue(cond3.getCondition());
+  }
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/InstantCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/InstantCommandTest.java
new file mode 100644
index 0000000..a109ed6
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/InstantCommandTest.java
@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class InstantCommandTest extends CommandTestBase {
+  @Test
+  void instantCommandScheduleTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    ConditionHolder cond = new ConditionHolder();
+
+    InstantCommand command = new InstantCommand(() -> cond.setCondition(true));
+
+    scheduler.schedule(command);
+    scheduler.run();
+
+    assertTrue(cond.getCondition());
+    assertFalse(scheduler.isScheduled(command));
+  }
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommandTest.java
new file mode 100644
index 0000000..2ac2129
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommandTest.java
@@ -0,0 +1,118 @@
+/*----------------------------------------------------------------------------*/
+/* 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.ArrayList;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.kinematics.MecanumDriveKinematics;
+import edu.wpi.first.wpilibj.kinematics.MecanumDriveOdometry;
+import edu.wpi.first.wpilibj.kinematics.MecanumDriveWheelSpeeds;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class MecanumControllerCommandTest {
+  private final Timer m_timer = new Timer();
+  private Rotation2d m_angle = new Rotation2d(0);
+
+  private double m_frontLeftSpeed;
+  private double m_rearLeftSpeed;
+  private double m_frontRightSpeed;
+  private double m_rearRightSpeed;
+
+  private final ProfiledPIDController m_rotController = new ProfiledPIDController(1, 0, 0,
+      new TrapezoidProfile.Constraints(3 * Math.PI, Math.PI));
+
+  private static final double kxTolerance = 1 / 12.0;
+  private static final double kyTolerance = 1 / 12.0;
+  private static final double kAngularTolerance = 1 / 12.0;
+
+  private static final double kWheelBase = 0.5;
+  private static final double kTrackWidth = 0.5;
+
+  private final MecanumDriveKinematics m_kinematics = new MecanumDriveKinematics(
+      new Translation2d(kWheelBase / 2, kTrackWidth / 2),
+      new Translation2d(kWheelBase / 2, -kTrackWidth / 2),
+      new Translation2d(-kWheelBase / 2, kTrackWidth / 2),
+      new Translation2d(-kWheelBase / 2, -kTrackWidth / 2));
+
+  private final MecanumDriveOdometry m_odometry = new MecanumDriveOdometry(m_kinematics,
+      new Rotation2d(0), new Pose2d(0, 0, new Rotation2d(0)));
+
+  public void setWheelSpeeds(MecanumDriveWheelSpeeds wheelSpeeds) {
+    this.m_frontLeftSpeed = wheelSpeeds.frontLeftMetersPerSecond;
+    this.m_rearLeftSpeed = wheelSpeeds.rearLeftMetersPerSecond;
+    this.m_frontRightSpeed = wheelSpeeds.frontRightMetersPerSecond;
+    this.m_rearRightSpeed = wheelSpeeds.rearRightMetersPerSecond;
+  }
+
+  public MecanumDriveWheelSpeeds getCurrentWheelSpeeds() {
+    return new MecanumDriveWheelSpeeds(m_frontLeftSpeed,
+      m_frontRightSpeed, m_rearLeftSpeed, m_rearRightSpeed);
+  }
+
+  public Pose2d getRobotPose() {
+    m_odometry.updateWithTime(m_timer.get(), m_angle, getCurrentWheelSpeeds());
+    return m_odometry.getPoseMeters();
+  }
+
+  @Test
+  @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
+  void testReachesReference() {
+    final var subsystem = new Subsystem() {};
+
+    final var waypoints = new ArrayList<Pose2d>();
+    waypoints.add(new Pose2d(0, 0, new Rotation2d(0)));
+    waypoints.add(new Pose2d(1, 5, new Rotation2d(3)));
+    var config = new TrajectoryConfig(8.8, 0.1);
+    final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config);
+
+    final var endState = trajectory.sample(trajectory.getTotalTimeSeconds());
+
+    final var command = new MecanumControllerCommand(trajectory,
+        this::getRobotPose,
+        m_kinematics,
+        new PIDController(0.6, 0, 0),
+        new PIDController(0.6, 0, 0),
+        m_rotController,
+        8.8,
+        this::setWheelSpeeds,
+        subsystem);
+
+    m_timer.reset();
+    m_timer.start();
+
+    command.initialize();
+    while (!command.isFinished()) {
+      command.execute();
+      m_angle = trajectory.sample(m_timer.get()).poseMeters.getRotation();
+    }
+    m_timer.stop();
+    command.end(true);
+
+    assertAll(
+        () -> assertEquals(endState.poseMeters.getTranslation().getX(),
+          getRobotPose().getTranslation().getX(), kxTolerance),
+        () -> assertEquals(endState.poseMeters.getTranslation().getY(),
+          getRobotPose().getTranslation().getY(), kyTolerance),
+        () -> assertEquals(endState.poseMeters.getRotation().getRadians(),
+          getRobotPose().getRotation().getRadians(), kAngularTolerance)
+    );
+  }
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/NotifierCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/NotifierCommandTest.java
new file mode 100644
index 0000000..fb06844
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/NotifierCommandTest.java
@@ -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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.api.condition.DisabledOnOs;
+import org.junit.jupiter.api.condition.OS;
+
+import edu.wpi.first.wpilibj.Timer;
+
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+@DisabledOnOs(OS.MAC)
+class NotifierCommandTest extends CommandTestBase {
+  @Test
+  void notifierCommandScheduleTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    Counter counter = new Counter();
+
+    NotifierCommand command = new NotifierCommand(counter::increment, 0.01);
+
+    scheduler.schedule(command);
+    Timer.delay(0.25);
+    scheduler.cancel(command);
+
+    assertTrue(counter.m_counter > 10, "Should have hit at least 10 triggers");
+    assertTrue(counter.m_counter < 100, "Shouldn't hit more then 100 triggers");
+  }
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroupTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroupTest.java
new file mode 100644
index 0000000..d03b4aa
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroupTest.java
@@ -0,0 +1,131 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.mockito.Mockito.never;
+import static org.mockito.Mockito.times;
+import static org.mockito.Mockito.verify;
+
+class ParallelCommandGroupTest extends CommandTestBase {
+  @Test
+  void parallelGroupScheduleTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true);
+    Command command2 = command2Holder.getMock();
+
+    Command group = new ParallelCommandGroup(command1, command2);
+
+    scheduler.schedule(group);
+
+    verify(command1).initialize();
+    verify(command2).initialize();
+
+    command1Holder.setFinished(true);
+    scheduler.run();
+    command2Holder.setFinished(true);
+    scheduler.run();
+
+    verify(command1).execute();
+    verify(command1).end(false);
+    verify(command2, times(2)).execute();
+    verify(command2).end(false);
+
+    assertFalse(scheduler.isScheduled(group));
+  }
+
+  @Test
+  void parallelGroupInterruptTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true);
+    Command command2 = command2Holder.getMock();
+
+    Command group = new ParallelCommandGroup(command1, command2);
+
+    scheduler.schedule(group);
+
+    command1Holder.setFinished(true);
+    scheduler.run();
+    scheduler.run();
+    scheduler.cancel(group);
+
+    verify(command1).execute();
+    verify(command1).end(false);
+    verify(command1, never()).end(true);
+
+    verify(command2, times(2)).execute();
+    verify(command2, never()).end(false);
+    verify(command2).end(true);
+
+    assertFalse(scheduler.isScheduled(group));
+  }
+
+  @Test
+  void notScheduledCancelTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true);
+    Command command2 = command2Holder.getMock();
+
+    Command group = new ParallelCommandGroup(command1, command2);
+
+    assertDoesNotThrow(() -> scheduler.cancel(group));
+  }
+
+  @Test
+  void parallelGroupRequirementTest() {
+    Subsystem system1 = new TestSubsystem();
+    Subsystem system2 = new TestSubsystem();
+    Subsystem system3 = new TestSubsystem();
+    Subsystem system4 = new TestSubsystem();
+
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true, system3);
+    Command command2 = command2Holder.getMock();
+    MockCommandHolder command3Holder = new MockCommandHolder(true, system3, system4);
+    Command command3 = command3Holder.getMock();
+
+    Command group = new ParallelCommandGroup(command1, command2);
+
+    scheduler.schedule(group);
+    scheduler.schedule(command3);
+
+    assertFalse(scheduler.isScheduled(group));
+    assertTrue(scheduler.isScheduled(command3));
+  }
+
+  @Test
+  void parallelGroupRequirementErrorTest() {
+    Subsystem system1 = new TestSubsystem();
+    Subsystem system2 = new TestSubsystem();
+    Subsystem system3 = new TestSubsystem();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true, system2, system3);
+    Command command2 = command2Holder.getMock();
+
+    assertThrows(IllegalArgumentException.class,
+        () -> new ParallelCommandGroup(command1, command2));
+  }
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroupTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroupTest.java
new file mode 100644
index 0000000..c7e3769
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroupTest.java
@@ -0,0 +1,129 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.mockito.Mockito.never;
+import static org.mockito.Mockito.verify;
+import static org.mockito.internal.verification.VerificationModeFactory.times;
+
+class ParallelDeadlineGroupTest extends CommandTestBase {
+  @Test
+  void parallelDeadlineScheduleTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true);
+    Command command2 = command2Holder.getMock();
+    command2Holder.setFinished(true);
+    MockCommandHolder command3Holder = new MockCommandHolder(true);
+    Command command3 = command3Holder.getMock();
+
+    Command group = new ParallelDeadlineGroup(command1, command2, command3);
+
+    scheduler.schedule(group);
+    scheduler.run();
+
+    assertTrue(scheduler.isScheduled(group));
+
+    command1Holder.setFinished(true);
+    scheduler.run();
+
+    assertFalse(scheduler.isScheduled(group));
+
+    verify(command2).initialize();
+    verify(command2).execute();
+    verify(command2).end(false);
+    verify(command2, never()).end(true);
+
+    verify(command1).initialize();
+    verify(command1, times(2)).execute();
+    verify(command1).end(false);
+    verify(command1, never()).end(true);
+
+    verify(command3).initialize();
+    verify(command3, times(2)).execute();
+    verify(command3, never()).end(false);
+    verify(command3).end(true);
+  }
+
+  @Test
+  void parallelDeadlineInterruptTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true);
+    Command command2 = command2Holder.getMock();
+    command2Holder.setFinished(true);
+
+    Command group = new ParallelDeadlineGroup(command1, command2);
+
+    scheduler.schedule(group);
+
+    scheduler.run();
+    scheduler.run();
+    scheduler.cancel(group);
+
+    verify(command1, times(2)).execute();
+    verify(command1, never()).end(false);
+    verify(command1).end(true);
+
+    verify(command2).execute();
+    verify(command2).end(false);
+    verify(command2, never()).end(true);
+
+    assertFalse(scheduler.isScheduled(group));
+  }
+
+
+  @Test
+  void parallelDeadlineRequirementTest() {
+    Subsystem system1 = new TestSubsystem();
+    Subsystem system2 = new TestSubsystem();
+    Subsystem system3 = new TestSubsystem();
+    Subsystem system4 = new TestSubsystem();
+
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true, system3);
+    Command command2 = command2Holder.getMock();
+    MockCommandHolder command3Holder = new MockCommandHolder(true, system3, system4);
+    Command command3 = command3Holder.getMock();
+
+    Command group = new ParallelDeadlineGroup(command1, command2);
+
+    scheduler.schedule(group);
+    scheduler.schedule(command3);
+
+    assertFalse(scheduler.isScheduled(group));
+    assertTrue(scheduler.isScheduled(command3));
+  }
+
+  @Test
+  void parallelDeadlineRequirementErrorTest() {
+    Subsystem system1 = new TestSubsystem();
+    Subsystem system2 = new TestSubsystem();
+    Subsystem system3 = new TestSubsystem();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true, system2, system3);
+    Command command2 = command2Holder.getMock();
+
+    assertThrows(IllegalArgumentException.class,
+        () -> new ParallelDeadlineGroup(command1, command2));
+  }
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroupTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroupTest.java
new file mode 100644
index 0000000..8a4781f
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroupTest.java
@@ -0,0 +1,163 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertNotNull;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.mockito.Mockito.never;
+import static org.mockito.Mockito.times;
+import static org.mockito.Mockito.verify;
+
+class ParallelRaceGroupTest extends CommandTestBase {
+  @Test
+  void parallelRaceScheduleTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true);
+    Command command2 = command2Holder.getMock();
+
+    Command group = new ParallelRaceGroup(command1, command2);
+
+    scheduler.schedule(group);
+
+    verify(command1).initialize();
+    verify(command2).initialize();
+
+    command1Holder.setFinished(true);
+    scheduler.run();
+    command2Holder.setFinished(true);
+    scheduler.run();
+
+    verify(command1).execute();
+    verify(command1).end(false);
+    verify(command2).execute();
+    verify(command2).end(true);
+    verify(command2, never()).end(false);
+
+    assertFalse(scheduler.isScheduled(group));
+  }
+
+  @Test
+  void parallelRaceInterruptTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true);
+    Command command2 = command2Holder.getMock();
+
+    Command group = new ParallelRaceGroup(command1, command2);
+
+    scheduler.schedule(group);
+
+    scheduler.run();
+    scheduler.run();
+    scheduler.cancel(group);
+
+    verify(command1, times(2)).execute();
+    verify(command1, never()).end(false);
+    verify(command1).end(true);
+
+    verify(command2, times(2)).execute();
+    verify(command2, never()).end(false);
+    verify(command2).end(true);
+
+    assertFalse(scheduler.isScheduled(group));
+  }
+
+  @Test
+  void notScheduledCancelTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true);
+    Command command2 = command2Holder.getMock();
+
+    Command group = new ParallelRaceGroup(command1, command2);
+
+    assertDoesNotThrow(() -> scheduler.cancel(group));
+  }
+
+
+  @Test
+  void parallelRaceRequirementTest() {
+    Subsystem system1 = new TestSubsystem();
+    Subsystem system2 = new TestSubsystem();
+    Subsystem system3 = new TestSubsystem();
+    Subsystem system4 = new TestSubsystem();
+
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true, system3);
+    Command command2 = command2Holder.getMock();
+    MockCommandHolder command3Holder = new MockCommandHolder(true, system3, system4);
+    Command command3 = command3Holder.getMock();
+
+    Command group = new ParallelRaceGroup(command1, command2);
+
+    scheduler.schedule(group);
+    scheduler.schedule(command3);
+
+    assertFalse(scheduler.isScheduled(group));
+    assertTrue(scheduler.isScheduled(command3));
+  }
+
+  @Test
+  void parallelRaceRequirementErrorTest() {
+    Subsystem system1 = new TestSubsystem();
+    Subsystem system2 = new TestSubsystem();
+    Subsystem system3 = new TestSubsystem();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true, system2, system3);
+    Command command2 = command2Holder.getMock();
+
+    assertThrows(IllegalArgumentException.class, () -> new ParallelRaceGroup(command1, command2));
+  }
+
+  @Test
+  void parallelRaceOnlyCallsEndOnceTest() {
+    Subsystem system1 = new TestSubsystem();
+    Subsystem system2 = new TestSubsystem();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true, system1);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true, system2);
+    Command command2 = command2Holder.getMock();
+    MockCommandHolder perpetualCommandHolder = new MockCommandHolder(true);
+    Command command3 = new PerpetualCommand(perpetualCommandHolder.getMock());
+
+    Command group1 = new SequentialCommandGroup(command1, command2);
+    assertNotNull(group1);
+    assertNotNull(command3);
+    Command group2 = new ParallelRaceGroup(group1, command3);
+
+    CommandScheduler scheduler = new CommandScheduler();
+
+    scheduler.schedule(group2);
+    scheduler.run();
+    command1Holder.setFinished(true);
+    scheduler.run();
+    command2Holder.setFinished(true);
+    // at this point the sequential group should be done
+    assertDoesNotThrow(() -> scheduler.run());
+
+  }
+
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/PerpetualCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/PerpetualCommandTest.java
new file mode 100644
index 0000000..baf037f
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/PerpetualCommandTest.java
@@ -0,0 +1,26 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class PerpetualCommandTest extends CommandTestBase {
+  @Test
+  void perpetualCommandScheduleTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    PerpetualCommand command = new PerpetualCommand(new InstantCommand());
+
+    scheduler.schedule(command);
+    scheduler.run();
+
+    assertTrue(scheduler.isScheduled(command));
+  }
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/PrintCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/PrintCommandTest.java
new file mode 100644
index 0000000..7484396
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/PrintCommandTest.java
@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.io.ByteArrayOutputStream;
+import java.io.PrintStream;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+
+class PrintCommandTest extends CommandTestBase {
+  @Test
+  void printCommandScheduleTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    final PrintStream originalOut = System.out;
+    ByteArrayOutputStream testOut = new ByteArrayOutputStream();
+    System.setOut(new PrintStream(testOut));
+
+    PrintCommand command = new PrintCommand("Test!");
+
+    scheduler.schedule(command);
+    scheduler.run();
+
+    assertFalse(scheduler.isScheduled(command));
+    assertEquals(testOut.toString(), "Test!" + System.lineSeparator());
+
+    System.setOut(originalOut);
+  }
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ProxyScheduleCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ProxyScheduleCommandTest.java
new file mode 100644
index 0000000..b566aae
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ProxyScheduleCommandTest.java
@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.mockito.Mockito.verify;
+
+class ProxyScheduleCommandTest extends CommandTestBase {
+  @Test
+  void proxyScheduleCommandScheduleTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+
+    ProxyScheduleCommand scheduleCommand = new ProxyScheduleCommand(command1);
+
+    scheduler.schedule(scheduleCommand);
+
+    verify(command1).schedule();
+  }
+
+  @Test
+  void proxyScheduleCommandEndTest() {
+    CommandScheduler scheduler = CommandScheduler.getInstance();
+
+    ConditionHolder cond = new ConditionHolder();
+
+    WaitUntilCommand command = new WaitUntilCommand(cond::getCondition);
+
+    ProxyScheduleCommand scheduleCommand = new ProxyScheduleCommand(command);
+
+    scheduler.schedule(scheduleCommand);
+
+    scheduler.run();
+    assertTrue(scheduler.isScheduled(scheduleCommand));
+
+    cond.setCondition(true);
+    scheduler.run();
+    scheduler.run();
+    assertFalse(scheduler.isScheduled(scheduleCommand));
+  }
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/RobotDisabledCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/RobotDisabledCommandTest.java
new file mode 100644
index 0000000..e862216
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/RobotDisabledCommandTest.java
@@ -0,0 +1,183 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.Map;
+
+import org.junit.jupiter.api.Test;
+
+import static edu.wpi.first.wpilibj2.command.CommandGroupBase.parallel;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class RobotDisabledCommandTest extends CommandTestBase {
+  @Test
+  void robotDisabledCommandCancelTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder holder = new MockCommandHolder(false);
+    Command mockCommand = holder.getMock();
+
+    scheduler.schedule(mockCommand);
+
+    assertTrue(scheduler.isScheduled(mockCommand));
+
+    setDSEnabled(false);
+
+    scheduler.run();
+
+    assertFalse(scheduler.isScheduled(mockCommand));
+
+    setDSEnabled(true);
+  }
+
+  @Test
+  void runWhenDisabledTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder holder = new MockCommandHolder(true);
+    Command mockCommand = holder.getMock();
+
+    scheduler.schedule(mockCommand);
+
+    assertTrue(scheduler.isScheduled(mockCommand));
+
+    setDSEnabled(false);
+
+    scheduler.run();
+
+    assertTrue(scheduler.isScheduled(mockCommand));
+  }
+
+  @Test
+  void sequentialGroupRunWhenDisabledTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true);
+    Command command2 = command2Holder.getMock();
+    MockCommandHolder command3Holder = new MockCommandHolder(true);
+    Command command3 = command3Holder.getMock();
+    MockCommandHolder command4Holder = new MockCommandHolder(false);
+    Command command4 = command4Holder.getMock();
+
+    Command runWhenDisabled = new SequentialCommandGroup(command1, command2);
+    Command dontRunWhenDisabled = new SequentialCommandGroup(command3, command4);
+
+    scheduler.schedule(runWhenDisabled);
+    scheduler.schedule(dontRunWhenDisabled);
+
+    setDSEnabled(false);
+
+    scheduler.run();
+
+    assertTrue(scheduler.isScheduled(runWhenDisabled));
+    assertFalse(scheduler.isScheduled(dontRunWhenDisabled));
+  }
+
+  @Test
+  void parallelGroupRunWhenDisabledTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true);
+    Command command2 = command2Holder.getMock();
+    MockCommandHolder command3Holder = new MockCommandHolder(true);
+    Command command3 = command3Holder.getMock();
+    MockCommandHolder command4Holder = new MockCommandHolder(false);
+    Command command4 = command4Holder.getMock();
+
+    Command runWhenDisabled = new ParallelCommandGroup(command1, command2);
+    Command dontRunWhenDisabled = new ParallelCommandGroup(command3, command4);
+
+    scheduler.schedule(runWhenDisabled);
+    scheduler.schedule(dontRunWhenDisabled);
+
+    setDSEnabled(false);
+
+    scheduler.run();
+
+    assertTrue(scheduler.isScheduled(runWhenDisabled));
+    assertFalse(scheduler.isScheduled(dontRunWhenDisabled));
+  }
+
+  @Test
+  void conditionalRunWhenDisabledTest() {
+    setDSEnabled(false);
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true);
+    Command command2 = command2Holder.getMock();
+    MockCommandHolder command3Holder = new MockCommandHolder(true);
+    Command command3 = command3Holder.getMock();
+    MockCommandHolder command4Holder = new MockCommandHolder(false);
+    Command command4 = command4Holder.getMock();
+
+    CommandScheduler scheduler = new CommandScheduler();
+
+    Command runWhenDisabled = new ConditionalCommand(command1, command2, () -> true);
+    Command dontRunWhenDisabled = new ConditionalCommand(command3, command4, () -> true);
+
+    scheduler.schedule(runWhenDisabled, dontRunWhenDisabled);
+
+    assertTrue(scheduler.isScheduled(runWhenDisabled));
+    assertFalse(scheduler.isScheduled(dontRunWhenDisabled));
+  }
+
+  @Test
+  void selectRunWhenDisabledTest() {
+    setDSEnabled(false);
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true);
+    Command command2 = command2Holder.getMock();
+    MockCommandHolder command3Holder = new MockCommandHolder(true);
+    Command command3 = command3Holder.getMock();
+    MockCommandHolder command4Holder = new MockCommandHolder(false);
+    Command command4 = command4Holder.getMock();
+
+    Command runWhenDisabled = new SelectCommand(Map.of(1, command1, 2, command2), () -> 1);
+    Command dontRunWhenDisabled = new SelectCommand(Map.of(1, command3, 2, command4), () -> 1);
+
+    CommandScheduler scheduler = new CommandScheduler();
+
+    scheduler.schedule(runWhenDisabled, dontRunWhenDisabled);
+
+    assertTrue(scheduler.isScheduled(runWhenDisabled));
+    assertFalse(scheduler.isScheduled(dontRunWhenDisabled));
+  }
+
+  @Test
+  void parallelConditionalRunWhenDisabledTest() {
+    setDSEnabled(false);
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true);
+    Command command2 = command2Holder.getMock();
+    MockCommandHolder command3Holder = new MockCommandHolder(true);
+    Command command3 = command3Holder.getMock();
+    MockCommandHolder command4Holder = new MockCommandHolder(false);
+    Command command4 = command4Holder.getMock();
+
+    CommandScheduler scheduler = new CommandScheduler();
+
+    Command runWhenDisabled = new ConditionalCommand(command1, command2, () -> true);
+    Command dontRunWhenDisabled = new ConditionalCommand(command3, command4, () -> true);
+
+    Command parallel = parallel(runWhenDisabled, dontRunWhenDisabled);
+
+    scheduler.schedule(parallel);
+
+    assertFalse(scheduler.isScheduled(runWhenDisabled));
+  }
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/RunCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/RunCommandTest.java
new file mode 100644
index 0000000..3ff8c3d
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/RunCommandTest.java
@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class RunCommandTest extends CommandTestBase {
+  @Test
+  void runCommandScheduleTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    Counter counter = new Counter();
+
+    RunCommand command = new RunCommand(counter::increment);
+
+    scheduler.schedule(command);
+    scheduler.run();
+    scheduler.run();
+    scheduler.run();
+
+    assertEquals(3, counter.m_counter);
+  }
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ScheduleCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ScheduleCommandTest.java
new file mode 100644
index 0000000..9041627
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ScheduleCommandTest.java
@@ -0,0 +1,47 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.mockito.Mockito.verify;
+
+class ScheduleCommandTest extends CommandTestBase {
+  @Test
+  void scheduleCommandScheduleTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true);
+    Command command2 = command2Holder.getMock();
+
+    ScheduleCommand scheduleCommand = new ScheduleCommand(command1, command2);
+
+    scheduler.schedule(scheduleCommand);
+
+    verify(command1).schedule();
+    verify(command2).schedule();
+  }
+
+  @Test
+  void scheduleCommandDuringRunTest() {
+    CommandScheduler scheduler = CommandScheduler.getInstance();
+
+    InstantCommand toSchedule = new InstantCommand();
+    ScheduleCommand scheduleCommand = new ScheduleCommand(toSchedule);
+    SequentialCommandGroup group =
+        new SequentialCommandGroup(new InstantCommand(), scheduleCommand);
+
+    scheduler.schedule(group);
+    scheduler.schedule(new InstantCommand().perpetually());
+    scheduler.run();
+    assertDoesNotThrow(scheduler::run);
+  }
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulerTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulerTest.java
new file mode 100644
index 0000000..1f110b6
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulerTest.java
@@ -0,0 +1,57 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class SchedulerTest extends CommandTestBase {
+  @Test
+  void schedulerLambdaTestNoInterrupt() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    Counter counter = new Counter();
+
+    scheduler.onCommandInitialize(command -> counter.increment());
+    scheduler.onCommandExecute(command -> counter.increment());
+    scheduler.onCommandFinish(command -> counter.increment());
+
+    scheduler.schedule(new InstantCommand());
+    scheduler.run();
+
+    assertEquals(counter.m_counter, 3);
+  }
+
+  @Test
+  void schedulerInterruptLambdaTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    Counter counter = new Counter();
+
+    scheduler.onCommandInterrupt(command -> counter.increment());
+
+    Command command = new WaitCommand(10);
+
+    scheduler.schedule(command);
+    scheduler.cancel(command);
+
+    assertEquals(counter.m_counter, 1);
+  }
+
+  @Test
+  void unregisterSubsystemTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    Subsystem system = new TestSubsystem();
+
+    scheduler.registerSubsystem(system);
+    assertDoesNotThrow(() -> scheduler.unregisterSubsystem(system));
+  }
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SelectCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SelectCommandTest.java
new file mode 100644
index 0000000..df52855
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SelectCommandTest.java
@@ -0,0 +1,108 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.Map;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.mockito.Mockito.never;
+import static org.mockito.Mockito.verify;
+
+class SelectCommandTest extends CommandTestBase {
+  @Test
+  void selectCommandTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+    command1Holder.setFinished(true);
+    MockCommandHolder command2Holder = new MockCommandHolder(true);
+    Command command2 = command2Holder.getMock();
+    MockCommandHolder command3Holder = new MockCommandHolder(true);
+    Command command3 = command3Holder.getMock();
+
+    SelectCommand selectCommand =
+        new SelectCommand(Map.ofEntries(
+            Map.entry("one", command1),
+            Map.entry("two", command2),
+            Map.entry("three", command3)),
+            () -> "one");
+
+    scheduler.schedule(selectCommand);
+    scheduler.run();
+
+    verify(command1).initialize();
+    verify(command1).execute();
+    verify(command1).end(false);
+
+    verify(command2, never()).initialize();
+    verify(command2, never()).execute();
+    verify(command2, never()).end(false);
+
+    verify(command3, never()).initialize();
+    verify(command3, never()).execute();
+    verify(command3, never()).end(false);
+  }
+
+  @Test
+  void selectCommandInvalidKeyTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+    command1Holder.setFinished(true);
+    MockCommandHolder command2Holder = new MockCommandHolder(true);
+    Command command2 = command2Holder.getMock();
+    MockCommandHolder command3Holder = new MockCommandHolder(true);
+    Command command3 = command3Holder.getMock();
+
+    SelectCommand selectCommand =
+        new SelectCommand(Map.ofEntries(
+            Map.entry("one", command1),
+            Map.entry("two", command2),
+            Map.entry("three", command3)),
+            () -> "four");
+
+    assertDoesNotThrow(() -> scheduler.schedule(selectCommand));
+  }
+
+
+  @Test
+  void selectCommandRequirementTest() {
+    Subsystem system1 = new TestSubsystem();
+    Subsystem system2 = new TestSubsystem();
+    Subsystem system3 = new TestSubsystem();
+    Subsystem system4 = new TestSubsystem();
+
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true, system3);
+    Command command2 = command2Holder.getMock();
+    MockCommandHolder command3Holder = new MockCommandHolder(true, system3, system4);
+    Command command3 = command3Holder.getMock();
+
+    SelectCommand selectCommand = new SelectCommand(
+        Map.ofEntries(Map.entry("one", command1), Map.entry("two", command2),
+            Map.entry("three", command3)), () -> "one");
+
+    scheduler.schedule(selectCommand);
+    scheduler.schedule(new InstantCommand(() -> {
+    }, system3));
+
+    assertFalse(scheduler.isScheduled(selectCommand));
+
+    verify(command1).end(true);
+    verify(command2, never()).end(true);
+    verify(command3, never()).end(true);
+  }
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroupTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroupTest.java
new file mode 100644
index 0000000..8582140
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroupTest.java
@@ -0,0 +1,128 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.mockito.Mockito.never;
+import static org.mockito.Mockito.verify;
+
+class SequentialCommandGroupTest extends CommandTestBase {
+  @Test
+  void sequentialGroupScheduleTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true);
+    Command command2 = command2Holder.getMock();
+
+    Command group = new SequentialCommandGroup(command1, command2);
+
+    scheduler.schedule(group);
+
+    verify(command1).initialize();
+    verify(command2, never()).initialize();
+
+    command1Holder.setFinished(true);
+    scheduler.run();
+
+    verify(command1).execute();
+    verify(command1).end(false);
+    verify(command2).initialize();
+    verify(command2, never()).execute();
+    verify(command2, never()).end(false);
+
+    command2Holder.setFinished(true);
+    scheduler.run();
+
+    verify(command1).execute();
+    verify(command1).end(false);
+    verify(command2).execute();
+    verify(command2).end(false);
+
+    assertFalse(scheduler.isScheduled(group));
+  }
+
+  @Test
+  void sequentialGroupInterruptTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true);
+    Command command2 = command2Holder.getMock();
+    MockCommandHolder command3Holder = new MockCommandHolder(true);
+    Command command3 = command3Holder.getMock();
+
+    Command group = new SequentialCommandGroup(command1, command2, command3);
+
+    scheduler.schedule(group);
+
+    command1Holder.setFinished(true);
+    scheduler.run();
+    scheduler.cancel(group);
+    scheduler.run();
+
+    verify(command1).execute();
+    verify(command1, never()).end(true);
+    verify(command1).end(false);
+    verify(command2, never()).execute();
+    verify(command2).end(true);
+    verify(command2, never()).end(false);
+    verify(command3, never()).initialize();
+    verify(command3, never()).execute();
+    verify(command3, never()).end(true);
+    verify(command3, never()).end(false);
+
+    assertFalse(scheduler.isScheduled(group));
+  }
+
+  @Test
+  void notScheduledCancelTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true);
+    Command command2 = command2Holder.getMock();
+
+    Command group = new SequentialCommandGroup(command1, command2);
+
+    assertDoesNotThrow(() -> scheduler.cancel(group));
+  }
+
+
+  @Test
+  void sequentialGroupRequirementTest() {
+    Subsystem system1 = new TestSubsystem();
+    Subsystem system2 = new TestSubsystem();
+    Subsystem system3 = new TestSubsystem();
+    Subsystem system4 = new TestSubsystem();
+
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true, system3);
+    Command command2 = command2Holder.getMock();
+    MockCommandHolder command3Holder = new MockCommandHolder(true, system3, system4);
+    Command command3 = command3Holder.getMock();
+
+    Command group = new SequentialCommandGroup(command1, command2);
+
+    scheduler.schedule(group);
+    scheduler.schedule(command3);
+
+    assertFalse(scheduler.isScheduled(group));
+    assertTrue(scheduler.isScheduled(command3));
+  }
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/StartEndCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/StartEndCommandTest.java
new file mode 100644
index 0000000..93921e0
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/StartEndCommandTest.java
@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class StartEndCommandTest extends CommandTestBase {
+  @Test
+  void startEndCommandScheduleTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    ConditionHolder cond1 = new ConditionHolder();
+    ConditionHolder cond2 = new ConditionHolder();
+
+    StartEndCommand command =
+        new StartEndCommand(() -> cond1.setCondition(true), () -> cond2.setCondition(true));
+
+    scheduler.schedule(command);
+    scheduler.run();
+
+    assertTrue(scheduler.isScheduled(command));
+
+    scheduler.cancel(command);
+
+    assertFalse(scheduler.isScheduled(command));
+    assertTrue(cond1.getCondition());
+    assertTrue(cond2.getCondition());
+  }
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommandTest.java
new file mode 100644
index 0000000..5209199
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommandTest.java
@@ -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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.ArrayList;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.wpilibj.kinematics.SwerveDriveOdometry;
+import edu.wpi.first.wpilibj.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class SwerveControllerCommandTest {
+  private final Timer m_timer = new Timer();
+  private Rotation2d m_angle = new Rotation2d(0);
+
+  private SwerveModuleState[] m_moduleStates = new SwerveModuleState[]{
+    new SwerveModuleState(0, new Rotation2d(0)),
+    new SwerveModuleState(0, new Rotation2d(0)),
+    new SwerveModuleState(0, new Rotation2d(0)),
+    new SwerveModuleState(0, new Rotation2d(0))};
+
+  private final ProfiledPIDController m_rotController = new ProfiledPIDController(1, 0, 0,
+      new TrapezoidProfile.Constraints(3 * Math.PI, Math.PI));
+
+  private static final double kxTolerance = 1 / 12.0;
+  private static final double kyTolerance = 1 / 12.0;
+  private static final double kAngularTolerance = 1 / 12.0;
+
+  private static final double kWheelBase = 0.5;
+  private static final double kTrackWidth = 0.5;
+
+  private final SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(
+      new Translation2d(kWheelBase / 2, kTrackWidth / 2),
+      new Translation2d(kWheelBase / 2, -kTrackWidth / 2),
+      new Translation2d(-kWheelBase / 2, kTrackWidth / 2),
+      new Translation2d(-kWheelBase / 2, -kTrackWidth / 2));
+
+  private final SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(m_kinematics,
+      new Rotation2d(0), new Pose2d(0, 0, new Rotation2d(0)));
+
+  @SuppressWarnings("PMD.ArrayIsStoredDirectly")
+  public void setModuleStates(SwerveModuleState[] moduleStates) {
+    this.m_moduleStates = moduleStates;
+  }
+
+  public Pose2d getRobotPose() {
+    m_odometry.updateWithTime(m_timer.get(), m_angle, m_moduleStates);
+    return m_odometry.getPoseMeters();
+  }
+
+  @Test
+  @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
+  void testReachesReference() {
+    final var subsystem = new Subsystem() {};
+
+    final var waypoints = new ArrayList<Pose2d>();
+    waypoints.add(new Pose2d(0, 0, new Rotation2d(0)));
+    waypoints.add(new Pose2d(1, 5, new Rotation2d(3)));
+    var config = new TrajectoryConfig(8.8, 0.1);
+    final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config);
+
+    final var endState = trajectory.sample(trajectory.getTotalTimeSeconds());
+
+    final var command = new SwerveControllerCommand(trajectory,
+        this::getRobotPose,
+        m_kinematics,
+        new PIDController(0.6, 0, 0),
+        new PIDController(0.6, 0, 0),
+        m_rotController,
+        this::setModuleStates,
+        subsystem);
+
+    m_timer.reset();
+    m_timer.start();
+
+    command.initialize();
+    while (!command.isFinished()) {
+      command.execute();
+      m_angle = trajectory.sample(m_timer.get()).poseMeters.getRotation();
+    }
+    m_timer.stop();
+    command.end(true);
+
+    assertAll(
+        () -> assertEquals(endState.poseMeters.getTranslation().getX(),
+          getRobotPose().getTranslation().getX(), kxTolerance),
+        () -> assertEquals(endState.poseMeters.getTranslation().getY(),
+          getRobotPose().getTranslation().getY(), kyTolerance),
+        () -> assertEquals(endState.poseMeters.getRotation().getRadians(),
+          getRobotPose().getRotation().getRadians(), kAngularTolerance)
+    );
+  }
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/WaitCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/WaitCommandTest.java
new file mode 100644
index 0000000..5a522af
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/WaitCommandTest.java
@@ -0,0 +1,67 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.Timer;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.mockito.ArgumentMatchers.anyDouble;
+import static org.mockito.Mockito.never;
+import static org.mockito.Mockito.verify;
+import static org.mockito.Mockito.when;
+
+class WaitCommandTest extends CommandTestBase {
+  @Test
+  void waitCommandTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    WaitCommand waitCommand = new WaitCommand(2);
+
+    scheduler.schedule(waitCommand);
+    scheduler.run();
+    Timer.delay(1);
+    scheduler.run();
+
+    assertTrue(scheduler.isScheduled(waitCommand));
+
+    Timer.delay(2);
+
+    scheduler.run();
+
+    assertFalse(scheduler.isScheduled(waitCommand));
+  }
+
+  @Test
+  void withTimeoutTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+    when(command1.withTimeout(anyDouble())).thenCallRealMethod();
+
+    Command timeout = command1.withTimeout(2);
+
+    scheduler.schedule(timeout);
+    scheduler.run();
+
+    verify(command1).initialize();
+    verify(command1).execute();
+    assertFalse(scheduler.isScheduled(command1));
+    assertTrue(scheduler.isScheduled(timeout));
+
+    Timer.delay(3);
+    scheduler.run();
+
+    verify(command1).end(true);
+    verify(command1, never()).end(false);
+    assertFalse(scheduler.isScheduled(timeout));
+  }
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/WaitUntilCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/WaitUntilCommandTest.java
new file mode 100644
index 0000000..a99a18c
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/WaitUntilCommandTest.java
@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class WaitUntilCommandTest extends CommandTestBase {
+  @Test
+  void waitUntilTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    ConditionHolder condition = new ConditionHolder();
+
+    Command command = new WaitUntilCommand(condition::getCondition);
+
+    scheduler.schedule(command);
+    scheduler.run();
+    assertTrue(scheduler.isScheduled(command));
+    condition.setCondition(true);
+    scheduler.run();
+    assertFalse(scheduler.isScheduled(command));
+  }
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/ButtonTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/ButtonTest.cpp
new file mode 100644
index 0000000..829f0d2
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/ButtonTest.cpp
@@ -0,0 +1,195 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "CommandTestBase.h"
+#include "frc2/command/CommandScheduler.h"
+#include "frc2/command/RunCommand.h"
+#include "frc2/command/WaitUntilCommand.h"
+#include "frc2/command/button/Trigger.h"
+#include "gtest/gtest.h"
+
+using namespace frc2;
+class ButtonTest : public CommandTestBase {};
+
+TEST_F(ButtonTest, WhenPressedTest) {
+  auto& scheduler = CommandScheduler::GetInstance();
+  bool finished = false;
+  bool pressed = false;
+
+  WaitUntilCommand command([&finished] { return finished; });
+
+  Trigger([&pressed] { return pressed; }).WhenActive(&command);
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+  pressed = true;
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+  finished = true;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(ButtonTest, WhenReleasedTest) {
+  auto& scheduler = CommandScheduler::GetInstance();
+  bool finished = false;
+  bool pressed = false;
+  WaitUntilCommand command([&finished] { return finished; });
+
+  pressed = true;
+  Trigger([&pressed] { return pressed; }).WhenInactive(&command);
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+  pressed = false;
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+  finished = true;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(ButtonTest, WhileHeldTest) {
+  auto& scheduler = CommandScheduler::GetInstance();
+  bool finished = false;
+  bool pressed = false;
+  WaitUntilCommand command([&finished] { return finished; });
+
+  pressed = false;
+  Trigger([&pressed] { return pressed; }).WhileActiveContinous(&command);
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+  pressed = true;
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+  finished = true;
+  scheduler.Run();
+  finished = false;
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+  pressed = false;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(ButtonTest, WhenHeldTest) {
+  auto& scheduler = CommandScheduler::GetInstance();
+  bool finished = false;
+  bool pressed = false;
+  WaitUntilCommand command([&finished] { return finished; });
+
+  pressed = false;
+  Trigger([&pressed] { return pressed; }).WhileActiveOnce(&command);
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+  pressed = true;
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+  finished = true;
+  scheduler.Run();
+  finished = false;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+
+  pressed = false;
+  Trigger([&pressed] { return pressed; }).WhileActiveOnce(&command);
+  pressed = true;
+  scheduler.Run();
+  pressed = false;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(ButtonTest, ToggleWhenPressedTest) {
+  auto& scheduler = CommandScheduler::GetInstance();
+  bool finished = false;
+  bool pressed = false;
+  WaitUntilCommand command([&finished] { return finished; });
+
+  pressed = false;
+  Trigger([&pressed] { return pressed; }).ToggleWhenActive(&command);
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+  pressed = true;
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+  pressed = false;
+  scheduler.Run();
+  pressed = true;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(ButtonTest, AndTest) {
+  auto& scheduler = CommandScheduler::GetInstance();
+  bool finished = false;
+  bool pressed1 = false;
+  bool pressed2 = false;
+  WaitUntilCommand command([&finished] { return finished; });
+
+  (Trigger([&pressed1] { return pressed1; }) &&
+   Trigger([&pressed2] { return pressed2; }))
+      .WhenActive(&command);
+  pressed1 = true;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+  pressed2 = true;
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(ButtonTest, OrTest) {
+  auto& scheduler = CommandScheduler::GetInstance();
+  bool finished = false;
+  bool pressed1 = false;
+  bool pressed2 = false;
+  WaitUntilCommand command1([&finished] { return finished; });
+  WaitUntilCommand command2([&finished] { return finished; });
+
+  (Trigger([&pressed1] { return pressed1; }) ||
+   Trigger([&pressed2] { return pressed2; }))
+      .WhenActive(&command1);
+  pressed1 = true;
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command1));
+
+  pressed1 = false;
+
+  (Trigger([&pressed1] { return pressed1; }) ||
+   Trigger([&pressed2] { return pressed2; }))
+      .WhenActive(&command2);
+  pressed2 = true;
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command2));
+}
+
+TEST_F(ButtonTest, NegateTest) {
+  auto& scheduler = CommandScheduler::GetInstance();
+  bool finished = false;
+  bool pressed = true;
+  WaitUntilCommand command([&finished] { return finished; });
+
+  (!Trigger([&pressed] { return pressed; })).WhenActive(&command);
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+  pressed = false;
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(ButtonTest, RValueButtonTest) {
+  auto& scheduler = CommandScheduler::GetInstance();
+  int counter = 0;
+  bool pressed = false;
+
+  RunCommand command([&counter] { counter++; }, {});
+
+  Trigger([&pressed] { return pressed; }).WhenActive(std::move(command));
+  scheduler.Run();
+  EXPECT_EQ(counter, 0);
+  pressed = true;
+  scheduler.Run();
+  EXPECT_EQ(counter, 1);
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp
new file mode 100644
index 0000000..5e876c4
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp
@@ -0,0 +1,101 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "CommandTestBase.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/ParallelRaceGroup.h"
+#include "frc2/command/PerpetualCommand.h"
+#include "frc2/command/RunCommand.h"
+#include "frc2/command/SequentialCommandGroup.h"
+
+using namespace frc2;
+class CommandDecoratorTest : public CommandTestBase {};
+
+TEST_F(CommandDecoratorTest, WithTimeoutTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  auto command = RunCommand([] {}, {}).WithTimeout(100_ms);
+
+  scheduler.Schedule(&command);
+
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+
+  std::this_thread::sleep_for(std::chrono::milliseconds(150));
+
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(CommandDecoratorTest, WithInterruptTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  bool finished = false;
+
+  auto command =
+      RunCommand([] {}, {}).WithInterrupt([&finished] { return finished; });
+
+  scheduler.Schedule(&command);
+
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+
+  finished = true;
+
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(CommandDecoratorTest, BeforeStartingTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  bool finished = false;
+
+  auto command = InstantCommand([] {}, {}).BeforeStarting(
+      [&finished] { finished = true; });
+
+  scheduler.Schedule(&command);
+
+  EXPECT_TRUE(finished);
+
+  scheduler.Run();
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(CommandDecoratorTest, AndThenTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  bool finished = false;
+
+  auto command =
+      InstantCommand([] {}, {}).AndThen([&finished] { finished = true; });
+
+  scheduler.Schedule(&command);
+
+  EXPECT_FALSE(finished);
+
+  scheduler.Run();
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+  EXPECT_TRUE(finished);
+}
+
+TEST_F(CommandDecoratorTest, PerpetuallyTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  auto command = InstantCommand([] {}, {}).Perpetually();
+
+  scheduler.Schedule(&command);
+
+  scheduler.Run();
+  scheduler.Run();
+
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandRequirementsTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandRequirementsTest.cpp
new file mode 100644
index 0000000..2600430
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandRequirementsTest.cpp
@@ -0,0 +1,84 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "CommandTestBase.h"
+#include "frc2/command/CommandScheduler.h"
+#include "frc2/command/ConditionalCommand.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/ParallelCommandGroup.h"
+#include "frc2/command/ParallelDeadlineGroup.h"
+#include "frc2/command/ParallelRaceGroup.h"
+#include "frc2/command/SelectCommand.h"
+#include "frc2/command/SequentialCommandGroup.h"
+
+using namespace frc2;
+class CommandRequirementsTest : public CommandTestBase {};
+
+TEST_F(CommandRequirementsTest, RequirementInterruptTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  TestSubsystem requirement;
+
+  MockCommand command1({&requirement});
+  MockCommand command2({&requirement});
+
+  EXPECT_CALL(command1, Initialize());
+  EXPECT_CALL(command1, Execute());
+  EXPECT_CALL(command1, End(true));
+  EXPECT_CALL(command1, End(false)).Times(0);
+
+  EXPECT_CALL(command2, Initialize());
+  EXPECT_CALL(command2, Execute());
+  EXPECT_CALL(command2, End(true)).Times(0);
+  EXPECT_CALL(command2, End(false)).Times(0);
+
+  scheduler.Schedule(&command1);
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command1));
+  scheduler.Schedule(&command2);
+  EXPECT_FALSE(scheduler.IsScheduled(&command1));
+  EXPECT_TRUE(scheduler.IsScheduled(&command2));
+  scheduler.Run();
+}
+
+TEST_F(CommandRequirementsTest, RequirementUninterruptibleTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  TestSubsystem requirement;
+
+  MockCommand command1({&requirement});
+  MockCommand command2({&requirement});
+
+  EXPECT_CALL(command1, Initialize());
+  EXPECT_CALL(command1, Execute()).Times(2);
+  EXPECT_CALL(command1, End(true)).Times(0);
+  EXPECT_CALL(command1, End(false)).Times(0);
+
+  EXPECT_CALL(command2, Initialize()).Times(0);
+  EXPECT_CALL(command2, Execute()).Times(0);
+  EXPECT_CALL(command2, End(true)).Times(0);
+  EXPECT_CALL(command2, End(false)).Times(0);
+
+  scheduler.Schedule(false, &command1);
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command1));
+  scheduler.Schedule(&command2);
+  EXPECT_TRUE(scheduler.IsScheduled(&command1));
+  EXPECT_FALSE(scheduler.IsScheduled(&command2));
+  scheduler.Run();
+}
+
+TEST_F(CommandRequirementsTest, DefaultCommandRequirementErrorTest) {
+  TestSubsystem requirement1;
+  ErrorConfirmer confirmer("require");
+
+  MockCommand command1;
+
+  requirement1.SetDefaultCommand(std::move(command1));
+
+  EXPECT_TRUE(requirement1.GetDefaultCommand() == NULL);
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp
new file mode 100644
index 0000000..6572a98
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp
@@ -0,0 +1,103 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "CommandTestBase.h"
+
+using namespace frc2;
+class CommandScheduleTest : public CommandTestBase {};
+
+TEST_F(CommandScheduleTest, InstantScheduleTest) {
+  CommandScheduler scheduler = GetScheduler();
+  MockCommand command;
+
+  EXPECT_CALL(command, Initialize());
+  EXPECT_CALL(command, Execute());
+  EXPECT_CALL(command, End(false));
+
+  command.SetFinished(true);
+  scheduler.Schedule(&command);
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(CommandScheduleTest, SingleIterationScheduleTest) {
+  CommandScheduler scheduler = GetScheduler();
+  MockCommand command;
+
+  EXPECT_CALL(command, Initialize());
+  EXPECT_CALL(command, Execute()).Times(2);
+  EXPECT_CALL(command, End(false));
+
+  scheduler.Schedule(&command);
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+  scheduler.Run();
+  command.SetFinished(true);
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(CommandScheduleTest, MultiScheduleTest) {
+  CommandScheduler scheduler = GetScheduler();
+  MockCommand command1;
+  MockCommand command2;
+  MockCommand command3;
+
+  EXPECT_CALL(command1, Initialize());
+  EXPECT_CALL(command1, Execute()).Times(2);
+  EXPECT_CALL(command1, End(false));
+
+  EXPECT_CALL(command2, Initialize());
+  EXPECT_CALL(command2, Execute()).Times(3);
+  EXPECT_CALL(command2, End(false));
+
+  EXPECT_CALL(command3, Initialize());
+  EXPECT_CALL(command3, Execute()).Times(4);
+  EXPECT_CALL(command3, End(false));
+
+  scheduler.Schedule(&command1);
+  scheduler.Schedule(&command2);
+  scheduler.Schedule(&command3);
+  EXPECT_TRUE(scheduler.IsScheduled({&command1, &command2, &command3}));
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled({&command1, &command2, &command3}));
+  command1.SetFinished(true);
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled({&command2, &command3}));
+  EXPECT_FALSE(scheduler.IsScheduled(&command1));
+  command2.SetFinished(true);
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command3));
+  EXPECT_FALSE(scheduler.IsScheduled({&command1, &command2}));
+  command3.SetFinished(true);
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled({&command1, &command2, &command3}));
+}
+
+TEST_F(CommandScheduleTest, SchedulerCancelTest) {
+  CommandScheduler scheduler = GetScheduler();
+  MockCommand command;
+
+  EXPECT_CALL(command, Initialize());
+  EXPECT_CALL(command, Execute());
+  EXPECT_CALL(command, End(false)).Times(0);
+  EXPECT_CALL(command, End(true));
+
+  scheduler.Schedule(&command);
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+  scheduler.Cancel(&command);
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(CommandScheduleTest, NotScheduledCancelTest) {
+  CommandScheduler scheduler = GetScheduler();
+  MockCommand command;
+
+  EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(&command));
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.cpp
new file mode 100644
index 0000000..0429c62
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.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 "CommandTestBase.h"
+
+using namespace frc2;
+
+CommandTestBase::CommandTestBase() {
+  auto& scheduler = CommandScheduler::GetInstance();
+  scheduler.CancelAll();
+  scheduler.Enable();
+  scheduler.ClearButtons();
+}
+
+CommandScheduler CommandTestBase::GetScheduler() { return CommandScheduler(); }
+
+void CommandTestBase::SetUp() {
+  HALSIM_SetDriverStationEnabled(true);
+  while (!HALSIM_GetDriverStationEnabled()) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(1));
+  }
+}
+
+void CommandTestBase::TearDown() {
+  CommandScheduler::GetInstance().ClearButtons();
+}
+
+void CommandTestBase::SetDSEnabled(bool enabled) {
+  HALSIM_SetDriverStationEnabled(enabled);
+  while (HALSIM_GetDriverStationEnabled() != static_cast<int>(enabled)) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(1));
+  }
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.h b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.h
new file mode 100644
index 0000000..8b22844
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.h
@@ -0,0 +1,102 @@
+/*----------------------------------------------------------------------------*/
+/* 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 <regex>
+#include <utility>
+
+#include <mockdata/MockHooks.h>
+
+#include "ErrorConfirmer.h"
+#include "frc2/command/CommandGroupBase.h"
+#include "frc2/command/CommandScheduler.h"
+#include "frc2/command/SetUtilities.h"
+#include "frc2/command/SubsystemBase.h"
+#include "gmock/gmock.h"
+#include "gtest/gtest.h"
+#include "make_vector.h"
+#include "simulation/DriverStationSim.h"
+
+namespace frc2 {
+class CommandTestBase : public ::testing::Test {
+ public:
+  CommandTestBase();
+
+  class TestSubsystem : public SubsystemBase {};
+
+ protected:
+  class MockCommand : public Command {
+   public:
+    MOCK_CONST_METHOD0(GetRequirements, wpi::SmallSet<Subsystem*, 4>());
+    MOCK_METHOD0(IsFinished, bool());
+    MOCK_CONST_METHOD0(RunsWhenDisabled, bool());
+    MOCK_METHOD0(Initialize, void());
+    MOCK_METHOD0(Execute, void());
+    MOCK_METHOD1(End, void(bool interrupted));
+
+    MockCommand() {
+      m_requirements = {};
+      EXPECT_CALL(*this, GetRequirements())
+          .WillRepeatedly(::testing::Return(m_requirements));
+      EXPECT_CALL(*this, IsFinished()).WillRepeatedly(::testing::Return(false));
+      EXPECT_CALL(*this, RunsWhenDisabled())
+          .WillRepeatedly(::testing::Return(true));
+    }
+
+    MockCommand(std::initializer_list<Subsystem*> requirements,
+                bool finished = false, bool runWhenDisabled = true) {
+      m_requirements.insert(requirements.begin(), requirements.end());
+      EXPECT_CALL(*this, GetRequirements())
+          .WillRepeatedly(::testing::Return(m_requirements));
+      EXPECT_CALL(*this, IsFinished())
+          .WillRepeatedly(::testing::Return(finished));
+      EXPECT_CALL(*this, RunsWhenDisabled())
+          .WillRepeatedly(::testing::Return(runWhenDisabled));
+    }
+
+    MockCommand(MockCommand&& other) {
+      EXPECT_CALL(*this, IsFinished())
+          .WillRepeatedly(::testing::Return(other.IsFinished()));
+      EXPECT_CALL(*this, RunsWhenDisabled())
+          .WillRepeatedly(::testing::Return(other.RunsWhenDisabled()));
+      std::swap(m_requirements, other.m_requirements);
+      EXPECT_CALL(*this, GetRequirements())
+          .WillRepeatedly(::testing::Return(m_requirements));
+    }
+
+    MockCommand(const MockCommand& other) : Command{} {}
+
+    void SetFinished(bool finished) {
+      EXPECT_CALL(*this, IsFinished())
+          .WillRepeatedly(::testing::Return(finished));
+    }
+
+    ~MockCommand() {
+      auto& scheduler = CommandScheduler::GetInstance();
+      scheduler.Cancel(this);
+    }
+
+   protected:
+    std::unique_ptr<Command> TransferOwnership() && {
+      return std::make_unique<MockCommand>(std::move(*this));
+    }
+
+   private:
+    wpi::SmallSet<Subsystem*, 4> m_requirements;
+  };
+
+  CommandScheduler GetScheduler();
+
+  void SetUp() override;
+
+  void TearDown() override;
+
+  void SetDSEnabled(bool enabled);
+};
+}  // namespace frc2
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/ConditionalCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/ConditionalCommandTest.cpp
new file mode 100644
index 0000000..927721a
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/ConditionalCommandTest.cpp
@@ -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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/ConditionalCommand.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/SelectCommand.h"
+
+using namespace frc2;
+class ConditionalCommandTest : public CommandTestBase {};
+
+TEST_F(ConditionalCommandTest, ConditionalCommandScheduleTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  std::unique_ptr<MockCommand> mock = std::make_unique<MockCommand>();
+  MockCommand* mockptr = mock.get();
+
+  EXPECT_CALL(*mock, Initialize());
+  EXPECT_CALL(*mock, Execute()).Times(2);
+  EXPECT_CALL(*mock, End(false));
+
+  ConditionalCommand conditional(
+      std::move(mock), std::make_unique<InstantCommand>(), [] { return true; });
+
+  scheduler.Schedule(&conditional);
+  scheduler.Run();
+  mockptr->SetFinished(true);
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(&conditional));
+}
+
+TEST_F(ConditionalCommandTest, ConditionalCommandRequirementTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  TestSubsystem requirement1;
+  TestSubsystem requirement2;
+  TestSubsystem requirement3;
+  TestSubsystem requirement4;
+
+  InstantCommand command1([] {}, {&requirement1, &requirement2});
+  InstantCommand command2([] {}, {&requirement3});
+  InstantCommand command3([] {}, {&requirement3, &requirement4});
+
+  ConditionalCommand conditional(std::move(command1), std::move(command2),
+                                 [] { return true; });
+  scheduler.Schedule(&conditional);
+  scheduler.Schedule(&command3);
+
+  EXPECT_TRUE(scheduler.IsScheduled(&command3));
+  EXPECT_FALSE(scheduler.IsScheduled(&conditional));
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/DefaultCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/DefaultCommandTest.cpp
new file mode 100644
index 0000000..b97cbb6
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/DefaultCommandTest.cpp
@@ -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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/RunCommand.h"
+
+using namespace frc2;
+class DefaultCommandTest : public CommandTestBase {};
+
+TEST_F(DefaultCommandTest, DefaultCommandScheduleTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  TestSubsystem subsystem;
+
+  RunCommand command1([] {}, {&subsystem});
+
+  scheduler.SetDefaultCommand(&subsystem, std::move(command1));
+  auto handle = scheduler.GetDefaultCommand(&subsystem);
+  scheduler.Run();
+
+  EXPECT_TRUE(scheduler.IsScheduled(handle));
+}
+
+TEST_F(DefaultCommandTest, DefaultCommandInterruptResumeTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  TestSubsystem subsystem;
+
+  RunCommand command1([] {}, {&subsystem});
+  RunCommand command2([] {}, {&subsystem});
+
+  scheduler.SetDefaultCommand(&subsystem, std::move(command1));
+  auto handle = scheduler.GetDefaultCommand(&subsystem);
+  scheduler.Run();
+  scheduler.Schedule(&command2);
+
+  EXPECT_TRUE(scheduler.IsScheduled(&command2));
+  EXPECT_FALSE(scheduler.IsScheduled(handle));
+
+  scheduler.Cancel(&command2);
+  scheduler.Run();
+
+  EXPECT_TRUE(scheduler.IsScheduled(handle));
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/ErrorConfirmer.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/ErrorConfirmer.cpp
new file mode 100644
index 0000000..2565a94
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/ErrorConfirmer.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 "ErrorConfirmer.h"
+
+ErrorConfirmer* ErrorConfirmer::instance;
+
+int32_t ErrorConfirmer::HandleError(HAL_Bool isError, int32_t errorCode,
+                                    HAL_Bool isLVCode, const char* details,
+                                    const char* location, const char* callStack,
+                                    HAL_Bool printMsg) {
+  if (std::regex_search(details, std::regex(instance->m_msg))) {
+    instance->ConfirmError();
+  }
+  return 1;
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/ErrorConfirmer.h b/wpilibNewCommands/src/test/native/cpp/frc2/command/ErrorConfirmer.h
new file mode 100644
index 0000000..011158c
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/ErrorConfirmer.h
@@ -0,0 +1,42 @@
+/*----------------------------------------------------------------------------*/
+/* 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 <regex>
+
+#include <mockdata/MockHooks.h>
+
+#include "gmock/gmock.h"
+#include "simulation/DriverStationSim.h"
+
+class ErrorConfirmer {
+ public:
+  explicit ErrorConfirmer(const char* msg) : m_msg(msg) {
+    if (instance != nullptr) return;
+    HALSIM_SetSendError(HandleError);
+    EXPECT_CALL(*this, ConfirmError());
+    instance = this;
+  }
+
+  ~ErrorConfirmer() {
+    HALSIM_SetSendError(nullptr);
+    instance = nullptr;
+  }
+
+  MOCK_METHOD0(ConfirmError, void());
+
+  const char* m_msg;
+
+  static int32_t HandleError(HAL_Bool isError, int32_t errorCode,
+                             HAL_Bool isLVCode, const char* details,
+                             const char* location, const char* callStack,
+                             HAL_Bool printMsg);
+
+ private:
+  static ErrorConfirmer* instance;
+};
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/FunctionalCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/FunctionalCommandTest.cpp
new file mode 100644
index 0000000..140d4fb
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/FunctionalCommandTest.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 "CommandTestBase.h"
+#include "frc2/command/FunctionalCommand.h"
+
+using namespace frc2;
+class FunctionalCommandTest : public CommandTestBase {};
+
+TEST_F(FunctionalCommandTest, FunctionalCommandScheduleTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  int counter = 0;
+  bool finished = false;
+
+  FunctionalCommand command(
+      [&counter] { counter++; }, [&counter] { counter++; },
+      [&counter](bool) { counter++; }, [&finished] { return finished; });
+
+  scheduler.Schedule(&command);
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+  finished = true;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+  EXPECT_EQ(4, counter);
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/InstantCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/InstantCommandTest.cpp
new file mode 100644
index 0000000..e91f677
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/InstantCommandTest.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 "CommandTestBase.h"
+#include "frc2/command/InstantCommand.h"
+
+using namespace frc2;
+class InstantCommandTest : public CommandTestBase {};
+
+TEST_F(InstantCommandTest, InstantCommandScheduleTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  int counter = 0;
+
+  InstantCommand command([&counter] { counter++; }, {});
+
+  scheduler.Schedule(&command);
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+  EXPECT_EQ(counter, 1);
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/MecanumControllerCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/MecanumControllerCommandTest.cpp
new file mode 100644
index 0000000..4eae880
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/MecanumControllerCommandTest.cpp
@@ -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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <frc2/Timer.h>
+#include <frc2/command/MecanumControllerCommand.h>
+#include <frc2/command/Subsystem.h>
+
+#include <iostream>
+
+#include <frc/controller/PIDController.h>
+#include <frc/controller/ProfiledPIDController.h>
+#include <frc/geometry/Rotation2d.h>
+#include <frc/geometry/Translation2d.h>
+#include <frc/kinematics/MecanumDriveKinematics.h>
+#include <frc/kinematics/MecanumDriveOdometry.h>
+#include <frc/trajectory/TrajectoryGenerator.h>
+#include <wpi/math>
+
+#include "gtest/gtest.h"
+
+#define EXPECT_NEAR_UNITS(val1, val2, eps) \
+  EXPECT_LE(units::math::abs(val1 - val2), eps)
+
+class MecanumControllerCommandTest : public ::testing::Test {
+  using radians_per_second_squared_t =
+      units::compound_unit<units::radians,
+                           units::inverse<units::squared<units::second>>>;
+
+ protected:
+  frc2::Timer m_timer;
+  frc::Rotation2d m_angle{0_rad};
+
+  units::meters_per_second_t m_frontLeftSpeed = 0.0_mps;
+  units::meters_per_second_t m_rearLeftSpeed = 0.0_mps;
+  units::meters_per_second_t m_frontRightSpeed = 0.0_mps;
+  units::meters_per_second_t m_rearRightSpeed = 0.0_mps;
+
+  frc::ProfiledPIDController<units::radians> m_rotController{
+      1, 0, 0,
+      frc::TrapezoidProfile<units::radians>::Constraints{
+          9_rad_per_s, units::unit_t<radians_per_second_squared_t>(3)}};
+
+  static constexpr units::meter_t kxTolerance{1 / 12.0};
+  static constexpr units::meter_t kyTolerance{1 / 12.0};
+  static constexpr units::radian_t kAngularTolerance{1 / 12.0};
+
+  static constexpr units::meter_t kWheelBase{0.5};
+  static constexpr units::meter_t kTrackWidth{0.5};
+
+  frc::MecanumDriveKinematics m_kinematics{
+      frc::Translation2d{kWheelBase / 2, kTrackWidth / 2},
+      frc::Translation2d{kWheelBase / 2, -kTrackWidth / 2},
+      frc::Translation2d{-kWheelBase / 2, kTrackWidth / 2},
+      frc::Translation2d{-kWheelBase / 2, -kTrackWidth / 2}};
+
+  frc::MecanumDriveOdometry m_odometry{m_kinematics, 0_rad,
+                                       frc::Pose2d{0_m, 0_m, 0_rad}};
+
+  frc::MecanumDriveWheelSpeeds getCurrentWheelSpeeds() {
+    return frc::MecanumDriveWheelSpeeds{m_frontLeftSpeed, m_frontRightSpeed,
+                                        m_rearLeftSpeed, m_rearRightSpeed};
+  }
+
+  frc::Pose2d getRobotPose() {
+    m_odometry.UpdateWithTime(m_timer.Get(), m_angle, getCurrentWheelSpeeds());
+    return m_odometry.GetPose();
+  }
+};
+
+TEST_F(MecanumControllerCommandTest, ReachesReference) {
+  frc2::Subsystem subsystem{};
+
+  auto waypoints =
+      std::vector{frc::Pose2d{0_m, 0_m, 0_rad}, frc::Pose2d{1_m, 5_m, 3_rad}};
+  auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+      waypoints, {8.8_mps, 0.1_mps_sq});
+
+  auto endState = trajectory.Sample(trajectory.TotalTime());
+
+  auto command = frc2::MecanumControllerCommand(
+      trajectory, [&]() { return getRobotPose(); }, m_kinematics,
+
+      frc2::PIDController(0.6, 0, 0), frc2::PIDController(0.6, 0, 0),
+      m_rotController, units::meters_per_second_t(8.8),
+      [&](units::meters_per_second_t frontLeft,
+          units::meters_per_second_t rearLeft,
+          units::meters_per_second_t frontRight,
+          units::meters_per_second_t rearRight) {
+        m_frontLeftSpeed = frontLeft;
+        m_rearLeftSpeed = rearLeft;
+        m_frontRightSpeed = frontRight;
+        m_rearRightSpeed = rearRight;
+      },
+      {&subsystem});
+
+  m_timer.Reset();
+  m_timer.Start();
+  command.Initialize();
+  while (!command.IsFinished()) {
+    command.Execute();
+    m_angle = trajectory.Sample(m_timer.Get()).pose.Rotation();
+  }
+  m_timer.Stop();
+  command.End(false);
+
+  EXPECT_NEAR_UNITS(endState.pose.Translation().X(),
+                    getRobotPose().Translation().X(), kxTolerance);
+  EXPECT_NEAR_UNITS(endState.pose.Translation().Y(),
+                    getRobotPose().Translation().Y(), kyTolerance);
+  EXPECT_NEAR_UNITS(endState.pose.Rotation().Radians(),
+                    getRobotPose().Rotation().Radians(), kAngularTolerance);
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/NotifierCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/NotifierCommandTest.cpp
new file mode 100644
index 0000000..a34c292
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/NotifierCommandTest.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 "CommandTestBase.h"
+#include "frc2/command/NotifierCommand.h"
+
+using namespace frc2;
+class NotifierCommandTest : public CommandTestBase {};
+
+#ifdef __APPLE__
+TEST_F(NotifierCommandTest, DISABLED_NotifierCommandScheduleTest) {
+#else
+TEST_F(NotifierCommandTest, NotifierCommandScheduleTest) {
+#endif
+  CommandScheduler scheduler = GetScheduler();
+
+  int counter = 0;
+
+  NotifierCommand command([&counter] { counter++; }, 0.01_s, {});
+
+  scheduler.Schedule(&command);
+  std::this_thread::sleep_for(std::chrono::milliseconds(250));
+  scheduler.Cancel(&command);
+
+  EXPECT_GT(counter, 10);
+  EXPECT_LT(counter, 100);
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelCommandGroupTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelCommandGroupTest.cpp
new file mode 100644
index 0000000..55c7b98
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelCommandGroupTest.cpp
@@ -0,0 +1,120 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "CommandTestBase.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/ParallelCommandGroup.h"
+#include "frc2/command/WaitUntilCommand.h"
+
+using namespace frc2;
+class ParallelCommandGroupTest : public CommandTestBase {};
+
+TEST_F(ParallelCommandGroupTest, ParallelGroupScheduleTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
+  std::unique_ptr<MockCommand> command2Holder = std::make_unique<MockCommand>();
+
+  MockCommand* command1 = command1Holder.get();
+  MockCommand* command2 = command2Holder.get();
+
+  ParallelCommandGroup group(tcb::make_vector<std::unique_ptr<Command>>(
+      std::move(command1Holder), std::move(command2Holder)));
+
+  EXPECT_CALL(*command1, Initialize());
+  EXPECT_CALL(*command1, Execute()).Times(1);
+  EXPECT_CALL(*command1, End(false));
+
+  EXPECT_CALL(*command2, Initialize());
+  EXPECT_CALL(*command2, Execute()).Times(2);
+  EXPECT_CALL(*command2, End(false));
+
+  scheduler.Schedule(&group);
+
+  command1->SetFinished(true);
+  scheduler.Run();
+  command2->SetFinished(true);
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(ParallelCommandGroupTest, ParallelGroupInterruptTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
+  std::unique_ptr<MockCommand> command2Holder = std::make_unique<MockCommand>();
+
+  MockCommand* command1 = command1Holder.get();
+  MockCommand* command2 = command2Holder.get();
+
+  ParallelCommandGroup group(tcb::make_vector<std::unique_ptr<Command>>(
+      std::move(command1Holder), std::move(command2Holder)));
+
+  EXPECT_CALL(*command1, Initialize());
+  EXPECT_CALL(*command1, Execute()).Times(1);
+  EXPECT_CALL(*command1, End(false));
+
+  EXPECT_CALL(*command2, Initialize());
+  EXPECT_CALL(*command2, Execute()).Times(2);
+  EXPECT_CALL(*command2, End(false)).Times(0);
+  EXPECT_CALL(*command2, End(true));
+
+  scheduler.Schedule(&group);
+
+  command1->SetFinished(true);
+  scheduler.Run();
+  scheduler.Run();
+  scheduler.Cancel(&group);
+
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(ParallelCommandGroupTest, ParallelGroupNotScheduledCancelTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  ParallelCommandGroup group((InstantCommand(), InstantCommand()));
+
+  EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(&group));
+}
+
+TEST_F(ParallelCommandGroupTest, ParallelGroupCopyTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  bool finished = false;
+
+  WaitUntilCommand command([&finished] { return finished; });
+
+  ParallelCommandGroup group(command);
+  scheduler.Schedule(&group);
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&group));
+  finished = true;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(ParallelCommandGroupTest, ParallelGroupRequirementTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  TestSubsystem requirement1;
+  TestSubsystem requirement2;
+  TestSubsystem requirement3;
+  TestSubsystem requirement4;
+
+  InstantCommand command1([] {}, {&requirement1, &requirement2});
+  InstantCommand command2([] {}, {&requirement3});
+  InstantCommand command3([] {}, {&requirement3, &requirement4});
+
+  ParallelCommandGroup group(std::move(command1), std::move(command2));
+
+  scheduler.Schedule(&group);
+  scheduler.Schedule(&command3);
+
+  EXPECT_TRUE(scheduler.IsScheduled(&command3));
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelDeadlineGroupTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelDeadlineGroupTest.cpp
new file mode 100644
index 0000000..6e3246f
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelDeadlineGroupTest.cpp
@@ -0,0 +1,136 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "CommandTestBase.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/ParallelDeadlineGroup.h"
+#include "frc2/command/WaitUntilCommand.h"
+
+using namespace frc2;
+class ParallelDeadlineGroupTest : public CommandTestBase {};
+
+TEST_F(ParallelDeadlineGroupTest, DeadlineGroupScheduleTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
+  std::unique_ptr<MockCommand> command2Holder = std::make_unique<MockCommand>();
+  std::unique_ptr<MockCommand> command3Holder = std::make_unique<MockCommand>();
+
+  MockCommand* command1 = command1Holder.get();
+  MockCommand* command2 = command2Holder.get();
+  MockCommand* command3 = command3Holder.get();
+
+  ParallelDeadlineGroup group(
+      std::move(command1Holder),
+      tcb::make_vector<std::unique_ptr<Command>>(std::move(command2Holder),
+                                                 std::move(command3Holder)));
+
+  EXPECT_CALL(*command1, Initialize());
+  EXPECT_CALL(*command1, Execute()).Times(2);
+  EXPECT_CALL(*command1, End(false));
+
+  EXPECT_CALL(*command2, Initialize());
+  EXPECT_CALL(*command2, Execute()).Times(1);
+  EXPECT_CALL(*command2, End(false));
+
+  EXPECT_CALL(*command3, Initialize());
+  EXPECT_CALL(*command3, Execute()).Times(2);
+  EXPECT_CALL(*command3, End(true));
+
+  scheduler.Schedule(&group);
+
+  command2->SetFinished(true);
+  scheduler.Run();
+  command1->SetFinished(true);
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(ParallelDeadlineGroupTest, SequentialGroupInterruptTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  TestSubsystem subsystem;
+
+  std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
+  std::unique_ptr<MockCommand> command2Holder = std::make_unique<MockCommand>();
+  std::unique_ptr<MockCommand> command3Holder = std::make_unique<MockCommand>();
+
+  MockCommand* command1 = command1Holder.get();
+  MockCommand* command2 = command2Holder.get();
+  MockCommand* command3 = command3Holder.get();
+
+  ParallelDeadlineGroup group(
+      std::move(command1Holder),
+      tcb::make_vector<std::unique_ptr<Command>>(std::move(command2Holder),
+                                                 std::move(command3Holder)));
+
+  EXPECT_CALL(*command1, Initialize());
+  EXPECT_CALL(*command1, Execute()).Times(1);
+  EXPECT_CALL(*command1, End(true));
+
+  EXPECT_CALL(*command2, Initialize());
+  EXPECT_CALL(*command2, Execute()).Times(1);
+  EXPECT_CALL(*command2, End(true));
+
+  EXPECT_CALL(*command3, Initialize());
+  EXPECT_CALL(*command3, Execute()).Times(1);
+  EXPECT_CALL(*command3, End(true));
+
+  scheduler.Schedule(&group);
+
+  scheduler.Run();
+  scheduler.Cancel(&group);
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(ParallelDeadlineGroupTest, DeadlineGroupNotScheduledCancelTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  ParallelDeadlineGroup group{InstantCommand(), InstantCommand()};
+
+  EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(&group));
+}
+
+TEST_F(ParallelDeadlineGroupTest, ParallelDeadlineCopyTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  bool finished = false;
+
+  WaitUntilCommand command([&finished] { return finished; });
+
+  ParallelDeadlineGroup group(command);
+  scheduler.Schedule(&group);
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&group));
+  finished = true;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(ParallelDeadlineGroupTest, ParallelDeadlineRequirementTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  TestSubsystem requirement1;
+  TestSubsystem requirement2;
+  TestSubsystem requirement3;
+  TestSubsystem requirement4;
+
+  InstantCommand command1([] {}, {&requirement1, &requirement2});
+  InstantCommand command2([] {}, {&requirement3});
+  InstantCommand command3([] {}, {&requirement3, &requirement4});
+
+  ParallelDeadlineGroup group(std::move(command1), std::move(command2));
+
+  scheduler.Schedule(&group);
+  scheduler.Schedule(&command3);
+
+  EXPECT_TRUE(scheduler.IsScheduled(&command3));
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelRaceGroupTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelRaceGroupTest.cpp
new file mode 100644
index 0000000..54762ca
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelRaceGroupTest.cpp
@@ -0,0 +1,156 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "CommandTestBase.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/ParallelRaceGroup.h"
+#include "frc2/command/SequentialCommandGroup.h"
+#include "frc2/command/WaitUntilCommand.h"
+
+using namespace frc2;
+class ParallelRaceGroupTest : public CommandTestBase {};
+
+TEST_F(ParallelRaceGroupTest, ParallelRaceScheduleTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
+  std::unique_ptr<MockCommand> command2Holder = std::make_unique<MockCommand>();
+  std::unique_ptr<MockCommand> command3Holder = std::make_unique<MockCommand>();
+
+  MockCommand* command1 = command1Holder.get();
+  MockCommand* command2 = command2Holder.get();
+  MockCommand* command3 = command3Holder.get();
+
+  ParallelRaceGroup group{tcb::make_vector<std::unique_ptr<Command>>(
+      std::move(command1Holder), std::move(command2Holder),
+      std::move(command3Holder))};
+
+  EXPECT_CALL(*command1, Initialize());
+  EXPECT_CALL(*command1, Execute()).Times(2);
+  EXPECT_CALL(*command1, End(true));
+
+  EXPECT_CALL(*command2, Initialize());
+  EXPECT_CALL(*command2, Execute()).Times(2);
+  EXPECT_CALL(*command2, End(false));
+
+  EXPECT_CALL(*command3, Initialize());
+  EXPECT_CALL(*command3, Execute()).Times(2);
+  EXPECT_CALL(*command3, End(true));
+
+  scheduler.Schedule(&group);
+
+  scheduler.Run();
+  command2->SetFinished(true);
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(ParallelRaceGroupTest, ParallelRaceInterruptTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
+  std::unique_ptr<MockCommand> command2Holder = std::make_unique<MockCommand>();
+  std::unique_ptr<MockCommand> command3Holder = std::make_unique<MockCommand>();
+
+  MockCommand* command1 = command1Holder.get();
+  MockCommand* command2 = command2Holder.get();
+  MockCommand* command3 = command3Holder.get();
+
+  ParallelRaceGroup group{tcb::make_vector<std::unique_ptr<Command>>(
+      std::move(command1Holder), std::move(command2Holder),
+      std::move(command3Holder))};
+
+  EXPECT_CALL(*command1, Initialize());
+  EXPECT_CALL(*command1, Execute()).Times(1);
+  EXPECT_CALL(*command1, End(true));
+
+  EXPECT_CALL(*command2, Initialize());
+  EXPECT_CALL(*command2, Execute()).Times(1);
+  EXPECT_CALL(*command2, End(true));
+
+  EXPECT_CALL(*command3, Initialize());
+  EXPECT_CALL(*command3, Execute()).Times(1);
+  EXPECT_CALL(*command3, End(true));
+
+  scheduler.Schedule(&group);
+
+  scheduler.Run();
+  scheduler.Cancel(&group);
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(ParallelRaceGroupTest, ParallelRaceNotScheduledCancelTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  ParallelRaceGroup group{InstantCommand(), InstantCommand()};
+
+  EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(&group));
+}
+
+TEST_F(ParallelRaceGroupTest, ParallelRaceCopyTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  bool finished = false;
+
+  WaitUntilCommand command([&finished] { return finished; });
+
+  ParallelRaceGroup group(command);
+  scheduler.Schedule(&group);
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&group));
+  finished = true;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(ParallelRaceGroupTest, RaceGroupRequirementTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  TestSubsystem requirement1;
+  TestSubsystem requirement2;
+  TestSubsystem requirement3;
+  TestSubsystem requirement4;
+
+  InstantCommand command1([] {}, {&requirement1, &requirement2});
+  InstantCommand command2([] {}, {&requirement3});
+  InstantCommand command3([] {}, {&requirement3, &requirement4});
+
+  ParallelRaceGroup group(std::move(command1), std::move(command2));
+
+  scheduler.Schedule(&group);
+  scheduler.Schedule(&command3);
+
+  EXPECT_TRUE(scheduler.IsScheduled(&command3));
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(ParallelRaceGroupTest, ParallelRaceOnlyCallsEndOnceTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  bool finished1 = false;
+  bool finished2 = false;
+  bool finished3 = false;
+
+  WaitUntilCommand command1([&finished1] { return finished1; });
+  WaitUntilCommand command2([&finished2] { return finished2; });
+  WaitUntilCommand command3([&finished3] { return finished3; });
+
+  SequentialCommandGroup group1(command1, command2);
+  ParallelRaceGroup group2(std::move(group1), command3);
+
+  scheduler.Schedule(&group2);
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&group2));
+  finished1 = true;
+  scheduler.Run();
+  finished2 = true;
+  EXPECT_NO_FATAL_FAILURE(scheduler.Run());
+  EXPECT_FALSE(scheduler.IsScheduled(&group2));
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/PerpetualCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/PerpetualCommandTest.cpp
new file mode 100644
index 0000000..b3ef861
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/PerpetualCommandTest.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 "CommandTestBase.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/PerpetualCommand.h"
+
+using namespace frc2;
+class PerpetualCommandTest : public CommandTestBase {};
+
+TEST_F(PerpetualCommandTest, PerpetualCommandScheduleTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  bool check = false;
+
+  PerpetualCommand command{InstantCommand([&check] { check = true; }, {})};
+
+  scheduler.Schedule(&command);
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+  EXPECT_TRUE(check);
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/PrintCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/PrintCommandTest.cpp
new file mode 100644
index 0000000..b940566
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/PrintCommandTest.cpp
@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <regex>
+
+#include "CommandTestBase.h"
+#include "frc2/command/PrintCommand.h"
+
+using namespace frc2;
+class PrintCommandTest : public CommandTestBase {};
+
+TEST_F(PrintCommandTest, PrintCommandScheduleTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  PrintCommand command("Test!");
+
+  testing::internal::CaptureStdout();
+
+  scheduler.Schedule(&command);
+  scheduler.Run();
+
+  EXPECT_TRUE(std::regex_search(testing::internal::GetCapturedStdout(),
+                                std::regex("Test!")));
+
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/ProxyScheduleCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/ProxyScheduleCommandTest.cpp
new file mode 100644
index 0000000..09a569f
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/ProxyScheduleCommandTest.cpp
@@ -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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <regex>
+
+#include "CommandTestBase.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/ProxyScheduleCommand.h"
+#include "frc2/command/WaitUntilCommand.h"
+
+using namespace frc2;
+class ProxyScheduleCommandTest : public CommandTestBase {};
+
+TEST_F(ProxyScheduleCommandTest, ProxyScheduleCommandScheduleTest) {
+  CommandScheduler& scheduler = CommandScheduler::GetInstance();
+
+  bool scheduled = false;
+
+  InstantCommand toSchedule([&scheduled] { scheduled = true; }, {});
+
+  ProxyScheduleCommand command(&toSchedule);
+
+  scheduler.Schedule(&command);
+  scheduler.Run();
+
+  EXPECT_TRUE(scheduled);
+}
+
+TEST_F(ProxyScheduleCommandTest, ProxyScheduleCommandEndTest) {
+  CommandScheduler& scheduler = CommandScheduler::GetInstance();
+
+  bool finished = false;
+
+  WaitUntilCommand toSchedule([&finished] { return finished; });
+
+  ProxyScheduleCommand command(&toSchedule);
+
+  scheduler.Schedule(&command);
+  scheduler.Run();
+
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+  finished = true;
+  scheduler.Run();
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/RobotDisabledCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/RobotDisabledCommandTest.cpp
new file mode 100644
index 0000000..bac40d5
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/RobotDisabledCommandTest.cpp
@@ -0,0 +1,156 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "CommandTestBase.h"
+#include "frc2/command/ConditionalCommand.h"
+#include "frc2/command/ParallelCommandGroup.h"
+#include "frc2/command/ParallelDeadlineGroup.h"
+#include "frc2/command/ParallelRaceGroup.h"
+#include "frc2/command/SelectCommand.h"
+#include "frc2/command/SequentialCommandGroup.h"
+
+using namespace frc2;
+class RobotDisabledCommandTest : public CommandTestBase {};
+
+TEST_F(RobotDisabledCommandTest, RobotDisabledCommandCancelTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  MockCommand command({}, false, false);
+
+  EXPECT_CALL(command, End(true));
+
+  SetDSEnabled(true);
+
+  scheduler.Schedule(&command);
+  scheduler.Run();
+
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+
+  SetDSEnabled(false);
+
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(RobotDisabledCommandTest, RunWhenDisabledTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  MockCommand command1;
+  MockCommand command2;
+
+  scheduler.Schedule(&command1);
+
+  SetDSEnabled(false);
+
+  scheduler.Run();
+
+  scheduler.Schedule(&command2);
+
+  EXPECT_TRUE(scheduler.IsScheduled(&command1));
+  EXPECT_TRUE(scheduler.IsScheduled(&command2));
+}
+
+TEST_F(RobotDisabledCommandTest, SequentialGroupRunWhenDisabledTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  SequentialCommandGroup runWhenDisabled{MockCommand(), MockCommand()};
+  SequentialCommandGroup dontRunWhenDisabled{MockCommand(),
+                                             MockCommand({}, false, false)};
+
+  SetDSEnabled(false);
+
+  scheduler.Schedule(&runWhenDisabled);
+  scheduler.Schedule(&dontRunWhenDisabled);
+
+  EXPECT_TRUE(scheduler.IsScheduled(&runWhenDisabled));
+  EXPECT_FALSE(scheduler.IsScheduled(&dontRunWhenDisabled));
+}
+
+TEST_F(RobotDisabledCommandTest, ParallelGroupRunWhenDisabledTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  ParallelCommandGroup runWhenDisabled{MockCommand(), MockCommand()};
+  ParallelCommandGroup dontRunWhenDisabled{MockCommand(),
+                                           MockCommand({}, false, false)};
+
+  SetDSEnabled(false);
+
+  scheduler.Schedule(&runWhenDisabled);
+  scheduler.Schedule(&dontRunWhenDisabled);
+
+  EXPECT_TRUE(scheduler.IsScheduled(&runWhenDisabled));
+  EXPECT_FALSE(scheduler.IsScheduled(&dontRunWhenDisabled));
+}
+
+TEST_F(RobotDisabledCommandTest, ParallelRaceRunWhenDisabledTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  ParallelRaceGroup runWhenDisabled{MockCommand(), MockCommand()};
+  ParallelRaceGroup dontRunWhenDisabled{MockCommand(),
+                                        MockCommand({}, false, false)};
+
+  SetDSEnabled(false);
+
+  scheduler.Schedule(&runWhenDisabled);
+  scheduler.Schedule(&dontRunWhenDisabled);
+
+  EXPECT_TRUE(scheduler.IsScheduled(&runWhenDisabled));
+  EXPECT_FALSE(scheduler.IsScheduled(&dontRunWhenDisabled));
+}
+
+TEST_F(RobotDisabledCommandTest, ParallelDeadlineRunWhenDisabledTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  ParallelDeadlineGroup runWhenDisabled{MockCommand(), MockCommand()};
+  ParallelDeadlineGroup dontRunWhenDisabled{MockCommand(),
+                                            MockCommand({}, false, false)};
+
+  SetDSEnabled(false);
+
+  scheduler.Schedule(&runWhenDisabled);
+  scheduler.Schedule(&dontRunWhenDisabled);
+
+  EXPECT_TRUE(scheduler.IsScheduled(&runWhenDisabled));
+  EXPECT_FALSE(scheduler.IsScheduled(&dontRunWhenDisabled));
+}
+
+TEST_F(RobotDisabledCommandTest, ConditionalCommandRunWhenDisabledTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  ConditionalCommand runWhenDisabled{MockCommand(), MockCommand(),
+                                     [] { return true; }};
+  ConditionalCommand dontRunWhenDisabled{
+      MockCommand(), MockCommand({}, false, false), [] { return true; }};
+
+  SetDSEnabled(false);
+
+  scheduler.Schedule(&runWhenDisabled);
+  scheduler.Schedule(&dontRunWhenDisabled);
+
+  EXPECT_TRUE(scheduler.IsScheduled(&runWhenDisabled));
+  EXPECT_FALSE(scheduler.IsScheduled(&dontRunWhenDisabled));
+}
+
+TEST_F(RobotDisabledCommandTest, SelectCommandRunWhenDisabledTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  SelectCommand<int> runWhenDisabled{[] { return 1; },
+                                     std::pair(1, MockCommand()),
+                                     std::pair(1, MockCommand())};
+  SelectCommand<int> dontRunWhenDisabled{
+      [] { return 1; }, std::pair(1, MockCommand()),
+      std::pair(1, MockCommand({}, false, false))};
+
+  SetDSEnabled(false);
+
+  scheduler.Schedule(&runWhenDisabled);
+  scheduler.Schedule(&dontRunWhenDisabled);
+
+  EXPECT_TRUE(scheduler.IsScheduled(&runWhenDisabled));
+  EXPECT_FALSE(scheduler.IsScheduled(&dontRunWhenDisabled));
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/RunCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/RunCommandTest.cpp
new file mode 100644
index 0000000..07eecb3
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/RunCommandTest.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 "CommandTestBase.h"
+#include "frc2/command/RunCommand.h"
+
+using namespace frc2;
+class RunCommandTest : public CommandTestBase {};
+
+TEST_F(RunCommandTest, RunCommandScheduleTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  int counter = 0;
+
+  RunCommand command([&counter] { counter++; }, {});
+
+  scheduler.Schedule(&command);
+  scheduler.Run();
+  scheduler.Run();
+  scheduler.Run();
+
+  EXPECT_EQ(3, counter);
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/ScheduleCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/ScheduleCommandTest.cpp
new file mode 100644
index 0000000..29e8dc5
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/ScheduleCommandTest.cpp
@@ -0,0 +1,32 @@
+/*----------------------------------------------------------------------------*/
+/* 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 <regex>
+
+#include "CommandTestBase.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/ScheduleCommand.h"
+#include "frc2/command/SequentialCommandGroup.h"
+
+using namespace frc2;
+class ScheduleCommandTest : public CommandTestBase {};
+
+TEST_F(ScheduleCommandTest, ScheduleCommandScheduleTest) {
+  CommandScheduler& scheduler = CommandScheduler::GetInstance();
+
+  bool scheduled = false;
+
+  InstantCommand toSchedule([&scheduled] { scheduled = true; }, {});
+
+  ScheduleCommand command(&toSchedule);
+
+  scheduler.Schedule(&command);
+  scheduler.Run();
+
+  EXPECT_TRUE(scheduled);
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/SchedulerTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/SchedulerTest.cpp
new file mode 100644
index 0000000..f0198c9
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/SchedulerTest.cpp
@@ -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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/RunCommand.h"
+
+using namespace frc2;
+class SchedulerTest : public CommandTestBase {};
+
+TEST_F(SchedulerTest, SchedulerLambdaTestNoInterrupt) {
+  CommandScheduler scheduler = GetScheduler();
+
+  InstantCommand command;
+
+  int counter = 0;
+
+  scheduler.OnCommandInitialize([&counter](const Command&) { counter++; });
+  scheduler.OnCommandExecute([&counter](const Command&) { counter++; });
+  scheduler.OnCommandFinish([&counter](const Command&) { counter++; });
+
+  scheduler.Schedule(&command);
+  scheduler.Run();
+
+  EXPECT_EQ(counter, 3);
+}
+
+TEST_F(SchedulerTest, SchedulerLambdaInterruptTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  RunCommand command([] {}, {});
+
+  int counter = 0;
+
+  scheduler.OnCommandInterrupt([&counter](const Command&) { counter++; });
+
+  scheduler.Schedule(&command);
+  scheduler.Run();
+  scheduler.Cancel(&command);
+
+  EXPECT_EQ(counter, 1);
+}
+
+TEST_F(SchedulerTest, UnregisterSubsystemTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  TestSubsystem system;
+
+  scheduler.RegisterSubsystem(&system);
+
+  EXPECT_NO_FATAL_FAILURE(scheduler.UnregisterSubsystem(&system));
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/SelectCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/SelectCommandTest.cpp
new file mode 100644
index 0000000..6be14ef
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/SelectCommandTest.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 "CommandTestBase.h"
+#include "frc2/command/ConditionalCommand.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/SelectCommand.h"
+
+using namespace frc2;
+class SelectCommandTest : public CommandTestBase {};
+
+TEST_F(SelectCommandTest, SelectCommandTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  std::unique_ptr<MockCommand> mock = std::make_unique<MockCommand>();
+  MockCommand* mockptr = mock.get();
+
+  EXPECT_CALL(*mock, Initialize());
+  EXPECT_CALL(*mock, Execute()).Times(2);
+  EXPECT_CALL(*mock, End(false));
+
+  std::vector<std::pair<int, std::unique_ptr<Command>>> temp;
+
+  temp.emplace_back(std::pair(1, std::move(mock)));
+  temp.emplace_back(std::pair(2, std::make_unique<InstantCommand>()));
+  temp.emplace_back(std::pair(3, std::make_unique<InstantCommand>()));
+
+  SelectCommand<int> select([] { return 1; }, std::move(temp));
+
+  scheduler.Schedule(&select);
+  scheduler.Run();
+  mockptr->SetFinished(true);
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(&select));
+}
+
+TEST_F(SelectCommandTest, SelectCommandRequirementTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  TestSubsystem requirement1;
+  TestSubsystem requirement2;
+  TestSubsystem requirement3;
+  TestSubsystem requirement4;
+
+  InstantCommand command1([] {}, {&requirement1, &requirement2});
+  InstantCommand command2([] {}, {&requirement3});
+  InstantCommand command3([] {}, {&requirement3, &requirement4});
+
+  SelectCommand<int> select([] { return 1; }, std::pair(1, std::move(command1)),
+                            std::pair(2, std::move(command2)));
+
+  scheduler.Schedule(&select);
+  scheduler.Schedule(&command3);
+
+  EXPECT_TRUE(scheduler.IsScheduled(&command3));
+  EXPECT_FALSE(scheduler.IsScheduled(&select));
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/SequentialCommandGroupTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/SequentialCommandGroupTest.cpp
new file mode 100644
index 0000000..3a6f8d8
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/SequentialCommandGroupTest.cpp
@@ -0,0 +1,137 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "CommandTestBase.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/SequentialCommandGroup.h"
+#include "frc2/command/WaitUntilCommand.h"
+
+using namespace frc2;
+class SequentialCommandGroupTest : public CommandTestBase {};
+
+TEST_F(SequentialCommandGroupTest, SequentialGroupScheduleTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
+  std::unique_ptr<MockCommand> command2Holder = std::make_unique<MockCommand>();
+  std::unique_ptr<MockCommand> command3Holder = std::make_unique<MockCommand>();
+
+  MockCommand* command1 = command1Holder.get();
+  MockCommand* command2 = command2Holder.get();
+  MockCommand* command3 = command3Holder.get();
+
+  SequentialCommandGroup group{tcb::make_vector<std::unique_ptr<Command>>(
+      std::move(command1Holder), std::move(command2Holder),
+      std::move(command3Holder))};
+
+  EXPECT_CALL(*command1, Initialize());
+  EXPECT_CALL(*command1, Execute()).Times(1);
+  EXPECT_CALL(*command1, End(false));
+
+  EXPECT_CALL(*command2, Initialize());
+  EXPECT_CALL(*command2, Execute()).Times(1);
+  EXPECT_CALL(*command2, End(false));
+
+  EXPECT_CALL(*command3, Initialize());
+  EXPECT_CALL(*command3, Execute()).Times(1);
+  EXPECT_CALL(*command3, End(false));
+
+  scheduler.Schedule(&group);
+
+  command1->SetFinished(true);
+  scheduler.Run();
+  command2->SetFinished(true);
+  scheduler.Run();
+  command3->SetFinished(true);
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(SequentialCommandGroupTest, SequentialGroupInterruptTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
+  std::unique_ptr<MockCommand> command2Holder = std::make_unique<MockCommand>();
+  std::unique_ptr<MockCommand> command3Holder = std::make_unique<MockCommand>();
+
+  MockCommand* command1 = command1Holder.get();
+  MockCommand* command2 = command2Holder.get();
+  MockCommand* command3 = command3Holder.get();
+
+  SequentialCommandGroup group{tcb::make_vector<std::unique_ptr<Command>>(
+      std::move(command1Holder), std::move(command2Holder),
+      std::move(command3Holder))};
+
+  EXPECT_CALL(*command1, Initialize());
+  EXPECT_CALL(*command1, Execute()).Times(1);
+  EXPECT_CALL(*command1, End(false));
+
+  EXPECT_CALL(*command2, Initialize());
+  EXPECT_CALL(*command2, Execute()).Times(0);
+  EXPECT_CALL(*command2, End(false)).Times(0);
+  EXPECT_CALL(*command2, End(true));
+
+  EXPECT_CALL(*command3, Initialize()).Times(0);
+  EXPECT_CALL(*command3, Execute()).Times(0);
+  EXPECT_CALL(*command3, End(false)).Times(0);
+  EXPECT_CALL(*command3, End(true)).Times(0);
+
+  scheduler.Schedule(&group);
+
+  command1->SetFinished(true);
+  scheduler.Run();
+  scheduler.Cancel(&group);
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(SequentialCommandGroupTest, SequentialGroupNotScheduledCancelTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  SequentialCommandGroup group{InstantCommand(), InstantCommand()};
+
+  EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(&group));
+}
+
+TEST_F(SequentialCommandGroupTest, SequentialGroupCopyTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  bool finished = false;
+
+  WaitUntilCommand command([&finished] { return finished; });
+
+  SequentialCommandGroup group(command);
+  scheduler.Schedule(&group);
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&group));
+  finished = true;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(SequentialCommandGroupTest, SequentialGroupRequirementTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  TestSubsystem requirement1;
+  TestSubsystem requirement2;
+  TestSubsystem requirement3;
+  TestSubsystem requirement4;
+
+  InstantCommand command1([] {}, {&requirement1, &requirement2});
+  InstantCommand command2([] {}, {&requirement3});
+  InstantCommand command3([] {}, {&requirement3, &requirement4});
+
+  SequentialCommandGroup group(std::move(command1), std::move(command2));
+
+  scheduler.Schedule(&group);
+  scheduler.Schedule(&command3);
+
+  EXPECT_TRUE(scheduler.IsScheduled(&command3));
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/StartEndCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/StartEndCommandTest.cpp
new file mode 100644
index 0000000..3f25792
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/StartEndCommandTest.cpp
@@ -0,0 +1,28 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "CommandTestBase.h"
+#include "frc2/command/StartEndCommand.h"
+
+using namespace frc2;
+class StartEndCommandTest : public CommandTestBase {};
+
+TEST_F(StartEndCommandTest, StartEndCommandScheduleTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  int counter = 0;
+
+  StartEndCommand command([&counter] { counter++; }, [&counter] { counter++; },
+                          {});
+
+  scheduler.Schedule(&command);
+  scheduler.Run();
+  scheduler.Run();
+  scheduler.Cancel(&command);
+
+  EXPECT_EQ(2, counter);
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/SwerveControllerCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/SwerveControllerCommandTest.cpp
new file mode 100644
index 0000000..fa0838d
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/SwerveControllerCommandTest.cpp
@@ -0,0 +1,106 @@
+/*----------------------------------------------------------------------------*/
+/* 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/Timer.h>
+#include <frc2/command/Subsystem.h>
+#include <frc2/command/SwerveControllerCommand.h>
+
+#include <iostream>
+
+#include <frc/controller/PIDController.h>
+#include <frc/controller/ProfiledPIDController.h>
+#include <frc/geometry/Rotation2d.h>
+#include <frc/geometry/Translation2d.h>
+#include <frc/kinematics/SwerveDriveKinematics.h>
+#include <frc/kinematics/SwerveDriveOdometry.h>
+#include <frc/kinematics/SwerveModuleState.h>
+#include <frc/trajectory/TrajectoryGenerator.h>
+#include <wpi/math>
+
+#include "gtest/gtest.h"
+
+#define EXPECT_NEAR_UNITS(val1, val2, eps) \
+  EXPECT_LE(units::math::abs(val1 - val2), eps)
+
+class SwerveControllerCommandTest : public ::testing::Test {
+  using radians_per_second_squared_t =
+      units::compound_unit<units::radians,
+                           units::inverse<units::squared<units::second>>>;
+
+ protected:
+  frc2::Timer m_timer;
+  frc::Rotation2d m_angle{0_rad};
+
+  std::array<frc::SwerveModuleState, 4> m_moduleStates{
+      frc::SwerveModuleState{}, frc::SwerveModuleState{},
+      frc::SwerveModuleState{}, frc::SwerveModuleState{}};
+
+  frc::ProfiledPIDController<units::radians> m_rotController{
+      1, 0, 0,
+      frc::TrapezoidProfile<units::radians>::Constraints{
+          9_rad_per_s, units::unit_t<radians_per_second_squared_t>(3)}};
+
+  static constexpr units::meter_t kxTolerance{1 / 12.0};
+  static constexpr units::meter_t kyTolerance{1 / 12.0};
+  static constexpr units::radian_t kAngularTolerance{1 / 12.0};
+
+  static constexpr units::meter_t kWheelBase{0.5};
+  static constexpr units::meter_t kTrackWidth{0.5};
+
+  frc::SwerveDriveKinematics<4> m_kinematics{
+      frc::Translation2d{kWheelBase / 2, kTrackWidth / 2},
+      frc::Translation2d{kWheelBase / 2, -kTrackWidth / 2},
+      frc::Translation2d{-kWheelBase / 2, kTrackWidth / 2},
+      frc::Translation2d{-kWheelBase / 2, -kTrackWidth / 2}};
+
+  frc::SwerveDriveOdometry<4> m_odometry{m_kinematics, 0_rad,
+                                         frc::Pose2d{0_m, 0_m, 0_rad}};
+
+  std::array<frc::SwerveModuleState, 4> getCurrentWheelSpeeds() {
+    return m_moduleStates;
+  }
+
+  frc::Pose2d getRobotPose() {
+    m_odometry.UpdateWithTime(m_timer.Get(), m_angle, getCurrentWheelSpeeds());
+    return m_odometry.GetPose();
+  }
+};
+
+TEST_F(SwerveControllerCommandTest, ReachesReference) {
+  frc2::Subsystem subsystem{};
+
+  auto waypoints =
+      std::vector{frc::Pose2d{0_m, 0_m, 0_rad}, frc::Pose2d{1_m, 5_m, 3_rad}};
+  auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+      waypoints, {8.8_mps, 0.1_mps_sq});
+
+  auto endState = trajectory.Sample(trajectory.TotalTime());
+
+  auto command = frc2::SwerveControllerCommand<4>(
+      trajectory, [&]() { return getRobotPose(); }, m_kinematics,
+
+      frc2::PIDController(0.6, 0, 0), frc2::PIDController(0.6, 0, 0),
+      m_rotController,
+      [&](auto moduleStates) { m_moduleStates = moduleStates; }, {&subsystem});
+
+  m_timer.Reset();
+  m_timer.Start();
+  command.Initialize();
+  while (!command.IsFinished()) {
+    command.Execute();
+    m_angle = trajectory.Sample(m_timer.Get()).pose.Rotation();
+  }
+  m_timer.Stop();
+  command.End(false);
+
+  EXPECT_NEAR_UNITS(endState.pose.Translation().X(),
+                    getRobotPose().Translation().X(), kxTolerance);
+  EXPECT_NEAR_UNITS(endState.pose.Translation().Y(),
+                    getRobotPose().Translation().Y(), kyTolerance);
+  EXPECT_NEAR_UNITS(endState.pose.Rotation().Radians(),
+                    getRobotPose().Rotation().Radians(), kAngularTolerance);
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/WaitCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/WaitCommandTest.cpp
new file mode 100644
index 0000000..75841a6
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/WaitCommandTest.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 "CommandTestBase.h"
+#include "frc2/command/WaitCommand.h"
+#include "frc2/command/WaitUntilCommand.h"
+
+using namespace frc2;
+class WaitCommandTest : public CommandTestBase {};
+
+TEST_F(WaitCommandTest, WaitCommandScheduleTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  WaitCommand command(100_ms);
+
+  scheduler.Schedule(&command);
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+  std::this_thread::sleep_for(std::chrono::milliseconds(110));
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/WaitUntilCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/WaitUntilCommandTest.cpp
new file mode 100644
index 0000000..e9728db
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/WaitUntilCommandTest.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 "CommandTestBase.h"
+#include "frc2/command/WaitUntilCommand.h"
+
+using namespace frc2;
+class WaitUntilCommandTest : public CommandTestBase {};
+
+TEST_F(WaitUntilCommandTest, WaitUntilCommandScheduleTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  bool finished = false;
+
+  WaitUntilCommand command([&finished] { return finished; });
+
+  scheduler.Schedule(&command);
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+  finished = true;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/make_vector.h b/wpilibNewCommands/src/test/native/cpp/frc2/command/make_vector.h
new file mode 100644
index 0000000..89d55b6
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/make_vector.h
@@ -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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <type_traits>
+#include <utility>
+#include <vector>
+
+namespace tcb {
+
+namespace detail {
+
+template <typename T, typename...>
+struct vec_type_helper {
+  using type = T;
+};
+
+template <typename... Args>
+struct vec_type_helper<void, Args...> {
+  using type = typename std::common_type_t<Args...>;
+};
+
+template <typename T, typename... Args>
+using vec_type_helper_t = typename vec_type_helper<T, Args...>::type;
+
+template <typename, typename...>
+struct all_constructible_and_convertible : std::true_type {};
+
+template <typename T, typename First, typename... Rest>
+struct all_constructible_and_convertible<T, First, Rest...>
+    : std::conditional_t<
+          std::is_constructible_v<T, First> && std::is_convertible_v<First, T>,
+          all_constructible_and_convertible<T, Rest...>, std::false_type> {};
+
+template <typename T, typename... Args,
+          typename std::enable_if_t<!std::is_trivially_copyable_v<T>, int> = 0>
+std::vector<T> make_vector_impl(Args&&... args) {
+  std::vector<T> vec;
+  vec.reserve(sizeof...(Args));
+  using arr_t = int[];
+  (void)arr_t{0, (vec.emplace_back(std::forward<Args>(args)), 0)...};
+  return vec;
+}
+
+template <typename T, typename... Args,
+          typename std::enable_if_t<std::is_trivially_copyable_v<T>, int> = 0>
+std::vector<T> make_vector_impl(Args&&... args) {
+  return std::vector<T>{std::forward<Args>(args)...};
+}
+
+}  // namespace detail
+
+template <
+    typename T = void, typename... Args,
+    typename V = detail::vec_type_helper_t<T, Args...>,
+    typename std::enable_if_t<
+        detail::all_constructible_and_convertible<V, Args...>::value, int> = 0>
+std::vector<V> make_vector(Args&&... args) {
+  return detail::make_vector_impl<V>(std::forward<Args>(args)...);
+}
+
+}  // namespace tcb
diff --git a/wpilibNewCommands/src/test/native/cpp/main.cpp b/wpilibNewCommands/src/test/native/cpp/main.cpp
new file mode 100644
index 0000000..c6b6c58
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/main.cpp
@@ -0,0 +1,17 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2015-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 <hal/HALBase.h>
+
+#include "gtest/gtest.h"
+
+int main(int argc, char** argv) {
+  HAL_Initialize(500, 0);
+  ::testing::InitGoogleTest(&argc, argv);
+  int ret = RUN_ALL_TESTS();
+  return ret;
+}
diff --git a/wpilibNewCommands/src/test/resources/META-INF/services/org.junit.jupiter.api.extension.Extension b/wpilibNewCommands/src/test/resources/META-INF/services/org.junit.jupiter.api.extension.Extension
new file mode 100644
index 0000000..daaab59
--- /dev/null
+++ b/wpilibNewCommands/src/test/resources/META-INF/services/org.junit.jupiter.api.extension.Extension
@@ -0,0 +1 @@
+edu.wpi.first.wpilibj2.MockHardwareExtension