Add new Java Command framework (#1682)

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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