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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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(.1_s);
+
+  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));
+}

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

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

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

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

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

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

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

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

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

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

wpilibNewCommands/src/test/native/cpp/frc2/command/NotifierCommandTest.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 "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_NEAR(.01 * counter, .25, .025);
+}

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

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

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

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

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

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

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

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

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

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

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

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

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

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(.1_s);
+
+  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));
+}

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

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

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

wpilibNewCommands/src/test/resources/META-INF/services/org.junit.jupiter.api.extension.Extension

@@ -0,0 +1 @@
+edu.wpi.first.wpilibj2.MockHardwareExtension