Fix cancel of inner commands in ConditionalCommands (#858)

wpilibc/src/main/native/cpp/Commands/Command.cpp

@@ -140,6 +140,7 @@ void Command::Removed() {
   m_initialized = false;
   m_canceled = false;
   m_running = false;
+  m_completed = true;
 }
 
 /**
@@ -156,6 +157,7 @@ void Command::Start() {
         CommandIllegalUse,
         "Can not start a command that is part of a command group");
 
+  m_completed = false;
   Scheduler::GetInstance()->AddCommand(this);
 }
 
@@ -211,13 +213,13 @@ void Command::End() {}
  */
 void Command::Interrupted() { End(); }
 
-void Command::_Initialize() {}
+void Command::_Initialize() { m_completed = false; }
 
-void Command::_Interrupted() {}
+void Command::_Interrupted() { m_completed = true; }
 
 void Command::_Execute() {}
 
-void Command::_End() {}
+void Command::_End() { m_completed = true; }
 
 /**
  * Called to indicate that the timer should start.
@@ -318,6 +320,7 @@ void Command::ClearRequirements() { m_requirements.clear(); }
 void Command::StartRunning() {
   m_running = true;
   m_startTime = -1;
+  m_completed = false;
 }
 
 /**
@@ -330,6 +333,20 @@ void Command::StartRunning() {
  */
 bool Command::IsRunning() const { return m_running; }
 
+/**
+ * Returns whether or not the command has been initialized.
+ *
+ * @return whether or not the command has been initialized.
+ */
+bool Command::IsInitialized() const { return m_initialized; }
+
+/**
+ * Returns whether or not the command has completed running.
+ *
+ * @return whether or not the command has completed running.
+ */
+bool Command::IsCompleted() const { return m_completed; }
+
 /**
  * This will cancel the current command.
  *

wpilibc/src/main/native/cpp/Commands/ConditionalCommand.cpp

@@ -7,6 +7,8 @@
 
 #include "Commands/ConditionalCommand.h"
 
+#include <iostream>
+
 #include "Commands/Scheduler.h"
 
 using namespace frc;
@@ -65,6 +67,7 @@ void ConditionalCommand::_Initialize() {
 
     m_chosenCommand->Start();
   }
+  Command::_Initialize();
 }
 
 void ConditionalCommand::_Cancel() {
@@ -76,14 +79,17 @@ void ConditionalCommand::_Cancel() {
 }
 
 bool ConditionalCommand::IsFinished() {
-  return m_chosenCommand != nullptr && m_chosenCommand->IsRunning() &&
-         m_chosenCommand->IsFinished();
+  if (m_chosenCommand != nullptr) {
+    return m_chosenCommand->IsCompleted();
+  } else {
+    return true;
+  }
 }
 
-void ConditionalCommand::Interrupted() {
+void ConditionalCommand::_Interrupted() {
   if (m_chosenCommand != nullptr && m_chosenCommand->IsRunning()) {
     m_chosenCommand->Cancel();
   }
 
-  Command::Interrupted();
+  Command::_Interrupted();
 }

wpilibc/src/main/native/include/Commands/Command.h

@@ -62,6 +62,8 @@ class Command : public ErrorBase, public SendableBase {
   bool Run();
   void Cancel();
   bool IsRunning() const;
+  bool IsInitialized() const;
+  bool IsCompleted() const;
   bool IsInterruptible() const;
   void SetInterruptible(bool interruptible);
   bool DoesRequire(Subsystem* subsystem) const;
@@ -148,6 +150,9 @@ class Command : public ErrorBase, public SendableBase {
   // The CommandGroup this is in
   CommandGroup* m_parent = nullptr;
 
+  // Whether or not this command has completed running
+  bool m_completed = false;
+
   int m_commandID = m_commandCounter++;
   static int m_commandCounter;
 

wpilibc/src/main/native/include/Commands/ConditionalCommand.h

@@ -10,23 +10,25 @@
 #include <llvm/Twine.h>
 
 #include "Commands/Command.h"
-#include "Commands/InstantCommand.h"
 
 namespace frc {
 
 /**
  * A ConditionalCommand is a Command that starts one of two commands.
  *
- * A ConditionalCommand uses m_condition to determine whether it should run
- * m_onTrue or m_onFalse.
+ * A ConditionalCommand uses the Condition method to determine whether it should
+ * run onTrue or onFalse.
  *
  * A ConditionalCommand adds the proper Command to the Scheduler during
  * Initialize() and then IsFinished() will return true once that Command has
  * finished executing.
  *
- * If no Command is specified for m_onFalse, the occurrence of that condition
+ * If no Command is specified for onFalse, the occurrence of that condition
  * will be a no-op.
  *
+ * A CondtionalCommand will require the superset of subsystems of the onTrue
+ * and onFalse commands.
+ *
  * @see Command
  * @see Scheduler
  */
@@ -48,7 +50,7 @@ class ConditionalCommand : public Command {
   void _Initialize() override;
   void _Cancel() override;
   bool IsFinished() override;
-  void Interrupted() override;
+  void _Interrupted() override;
 
  private:
   // The Command to execute if Condition() returns true

wpilibcIntegrationTests/src/FRCUserProgram/cpp/command/ConditionalCommandTest.cpp

@@ -10,6 +10,7 @@
 
 #include "Commands/ConditionalCommand.h"
 #include "Commands/Scheduler.h"
+#include "Commands/Subsystem.h"
 #include "command/MockCommand.h"
 #include "command/MockConditionalCommand.h"
 #include "gtest/gtest.h"
@@ -21,15 +22,19 @@ class ConditionalCommandTest : public testing::Test {
   MockConditionalCommand* m_command;
   MockCommand* m_onTrue;
   MockCommand* m_onFalse;
+  MockConditionalCommand* m_commandNull;
+  Subsystem* m_subsystem;
 
  protected:
   void SetUp() override {
     RobotState::SetImplementation(DriverStation::GetInstance());
     Scheduler::GetInstance()->SetEnabled(true);
 
-    m_onTrue = new MockCommand();
-    m_onFalse = new MockCommand();
+    m_subsystem = new Subsystem("MockSubsystem");
+    m_onTrue = new MockCommand(m_subsystem);
+    m_onFalse = new MockCommand(m_subsystem);
     m_command = new MockConditionalCommand(m_onTrue, m_onFalse);
+    m_commandNull = new MockConditionalCommand(m_onTrue, nullptr);
   }
 
   void TearDown() override { delete m_command; }
@@ -53,21 +58,58 @@ class ConditionalCommandTest : public testing::Test {
     EXPECT_EQ(end, command.GetEndCount());
     EXPECT_EQ(interrupted, command.GetInterruptedCount());
   }
+
+  void AssertConditionalCommandState(MockConditionalCommand& command,
+                                     int32_t initialize, int32_t execute,
+                                     int32_t isFinished, int32_t end,
+                                     int32_t interrupted) {
+    EXPECT_EQ(initialize, command.GetInitializeCount());
+    EXPECT_EQ(execute, command.GetExecuteCount());
+    EXPECT_EQ(isFinished, command.GetIsFinishedCount());
+    EXPECT_EQ(end, command.GetEndCount());
+    EXPECT_EQ(interrupted, command.GetInterruptedCount());
+  }
 };
 
 TEST_F(ConditionalCommandTest, OnTrueTest) {
   m_command->SetCondition(true);
 
+  SCOPED_TRACE("1");
   Scheduler::GetInstance()->AddCommand(m_command);
   AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
+  SCOPED_TRACE("2");
   Scheduler::GetInstance()->Run();  // init command and select m_onTrue
   AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
+  SCOPED_TRACE("3");
   Scheduler::GetInstance()->Run();  // init m_onTrue
   AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 1, 1, 0, 0);
+  SCOPED_TRACE("4");
   Scheduler::GetInstance()->Run();
-  AssertCommandState(*m_onTrue, 1, 1, 2, 0, 0);
+  AssertCommandState(*m_onTrue, 1, 1, 1, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 2, 2, 0, 0);
+  SCOPED_TRACE("5");
   Scheduler::GetInstance()->Run();
-  AssertCommandState(*m_onTrue, 1, 2, 4, 0, 0);
+  AssertCommandState(*m_onTrue, 1, 2, 2, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 3, 3, 0, 0);
+  SCOPED_TRACE("6");
+  m_onTrue->SetHasFinished(true);
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 3, 3, 1, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 4, 4, 1, 0);
+  SCOPED_TRACE("7");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 3, 3, 1, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 4, 4, 1, 0);
 
   EXPECT_TRUE(m_onTrue->GetInitializeCount() > 0)
       << "Did not initialize the true command\n";
@@ -80,16 +122,42 @@ TEST_F(ConditionalCommandTest, OnTrueTest) {
 TEST_F(ConditionalCommandTest, OnFalseTest) {
   m_command->SetCondition(false);
 
+  SCOPED_TRACE("1");
   Scheduler::GetInstance()->AddCommand(m_command);
   AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
+  SCOPED_TRACE("2");
   Scheduler::GetInstance()->Run();  // init command and select m_onTrue
   AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
+  SCOPED_TRACE("3");
   Scheduler::GetInstance()->Run();  // init m_onTrue
   AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 1, 1, 0, 0);
+  SCOPED_TRACE("4");
   Scheduler::GetInstance()->Run();
-  AssertCommandState(*m_onFalse, 1, 1, 2, 0, 0);
+  AssertCommandState(*m_onFalse, 1, 1, 1, 0, 0);
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 2, 2, 0, 0);
+  SCOPED_TRACE("5");
   Scheduler::GetInstance()->Run();
-  AssertCommandState(*m_onFalse, 1, 2, 4, 0, 0);
+  AssertCommandState(*m_onFalse, 1, 2, 2, 0, 0);
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 3, 3, 0, 0);
+  SCOPED_TRACE("6");
+  m_onFalse->SetHasFinished(true);
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onFalse, 1, 3, 3, 1, 0);
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 4, 4, 1, 0);
+  SCOPED_TRACE("7");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onFalse, 1, 3, 3, 1, 0);
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 4, 4, 1, 0);
 
   EXPECT_TRUE(m_onFalse->GetInitializeCount() > 0)
       << "Did not initialize the false command";
@@ -98,3 +166,272 @@ TEST_F(ConditionalCommandTest, OnFalseTest) {
 
   TeardownScheduler();
 }
+
+TEST_F(ConditionalCommandTest, CancelSubCommandTest) {
+  m_command->SetCondition(true);
+
+  SCOPED_TRACE("1");
+  Scheduler::GetInstance()->AddCommand(m_command);
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
+  SCOPED_TRACE("2");
+  Scheduler::GetInstance()->Run();  // init command and select m_onTrue
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
+  SCOPED_TRACE("3");
+  Scheduler::GetInstance()->Run();  // init m_onTrue
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 1, 1, 0, 0);
+  SCOPED_TRACE("4");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 1, 1, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 2, 2, 0, 0);
+  SCOPED_TRACE("5");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 2, 2, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 3, 3, 0, 0);
+  SCOPED_TRACE("6");
+  m_onTrue->Cancel();
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 2, 2, 0, 1);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 4, 4, 1, 0);
+  SCOPED_TRACE("7");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 2, 2, 0, 1);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 4, 4, 1, 0);
+
+  TeardownScheduler();
+}
+
+TEST_F(ConditionalCommandTest, CancelCondCommandTest) {
+  m_command->SetCondition(true);
+
+  SCOPED_TRACE("1");
+  Scheduler::GetInstance()->AddCommand(m_command);
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
+  SCOPED_TRACE("2");
+  Scheduler::GetInstance()->Run();  // init command and select m_onTrue
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
+  SCOPED_TRACE("3");
+  Scheduler::GetInstance()->Run();  // init m_onTrue
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 1, 1, 0, 0);
+  SCOPED_TRACE("4");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 1, 1, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 2, 2, 0, 0);
+  SCOPED_TRACE("5");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 2, 2, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 3, 3, 0, 0);
+  SCOPED_TRACE("6");
+  m_command->Cancel();
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 2, 2, 0, 1);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 3, 3, 0, 1);
+  SCOPED_TRACE("7");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 2, 2, 0, 1);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 3, 3, 0, 1);
+
+  TeardownScheduler();
+}
+
+TEST_F(ConditionalCommandTest, OnTrueTwiceTest) {
+  m_command->SetCondition(true);
+
+  SCOPED_TRACE("1");
+  Scheduler::GetInstance()->AddCommand(m_command);
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
+  SCOPED_TRACE("2");
+  Scheduler::GetInstance()->Run();  // init command and select m_onTrue
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
+  SCOPED_TRACE("3");
+  Scheduler::GetInstance()->Run();  // init m_onTrue
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 1, 1, 0, 0);
+  SCOPED_TRACE("4");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 1, 1, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 2, 2, 0, 0);
+  SCOPED_TRACE("5");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 2, 2, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 3, 3, 0, 0);
+  SCOPED_TRACE("6");
+  m_onTrue->SetHasFinished(true);
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 3, 3, 1, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 4, 4, 1, 0);
+  SCOPED_TRACE("7");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 3, 3, 1, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 4, 4, 1, 0);
+
+  m_onTrue->ResetCounters();
+  m_command->ResetCounters();
+  Scheduler::GetInstance()->AddCommand(m_command);
+
+  SCOPED_TRACE("11");
+  Scheduler::GetInstance()->AddCommand(m_command);
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
+  SCOPED_TRACE("12");
+  Scheduler::GetInstance()->Run();  // init command and select m_onTrue
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
+  SCOPED_TRACE("13");
+  Scheduler::GetInstance()->Run();  // init m_onTrue
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 1, 1, 0, 0);
+  SCOPED_TRACE("14");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 1, 1, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 2, 2, 0, 0);
+  SCOPED_TRACE("15");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 2, 2, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 3, 3, 0, 0);
+  SCOPED_TRACE("16");
+  m_onTrue->SetHasFinished(true);
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 3, 3, 1, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 4, 4, 1, 0);
+  SCOPED_TRACE("17");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 3, 3, 1, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 4, 4, 1, 0);
+
+  TeardownScheduler();
+}
+
+TEST_F(ConditionalCommandTest, OnTrueInstantTest) {
+  m_command->SetCondition(true);
+  m_onTrue->SetHasFinished(true);
+
+  SCOPED_TRACE("1");
+  Scheduler::GetInstance()->AddCommand(m_command);
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
+  SCOPED_TRACE("2");
+  Scheduler::GetInstance()->Run();  // init command and select m_onTrue
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
+  SCOPED_TRACE("3");
+  Scheduler::GetInstance()->Run();  // init m_onTrue
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 1, 1, 0, 0);
+  SCOPED_TRACE("4");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 1, 1, 1, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 2, 2, 1, 0);
+  SCOPED_TRACE("5");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 1, 1, 1, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 2, 2, 1, 0);
+
+  TeardownScheduler();
+}
+
+TEST_F(ConditionalCommandTest, CancelRequiresTest) {
+  m_command->SetCondition(true);
+
+  SCOPED_TRACE("1");
+  Scheduler::GetInstance()->AddCommand(m_command);
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
+  SCOPED_TRACE("2");
+  Scheduler::GetInstance()->Run();  // init command and select m_onTrue
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
+  SCOPED_TRACE("3");
+  Scheduler::GetInstance()->Run();  // init m_onTrue
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 1, 1, 0, 0);
+  SCOPED_TRACE("4");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 1, 1, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 2, 2, 0, 0);
+  SCOPED_TRACE("5");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 2, 2, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 3, 3, 0, 0);
+  SCOPED_TRACE("6");
+  m_onFalse->Start();
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 3, 3, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 4, 4, 0, 1);
+  SCOPED_TRACE("7");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 3, 3, 0, 1);
+  AssertCommandState(*m_onFalse, 1, 1, 1, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 4, 4, 0, 1);
+
+  TeardownScheduler();
+}
+
+TEST_F(ConditionalCommandTest, OnFalseNullTest) {
+  m_command->SetCondition(false);
+
+  SCOPED_TRACE("1");
+  Scheduler::GetInstance()->AddCommand(m_commandNull);
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_commandNull, 0, 0, 0, 0, 0);
+  SCOPED_TRACE("2");
+  Scheduler::GetInstance()->Run();  // init command and select m_onTrue
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_commandNull, 0, 0, 0, 0, 0);
+  SCOPED_TRACE("3");
+  Scheduler::GetInstance()->Run();  // init m_onTrue
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_commandNull, 1, 1, 1, 1, 0);
+  SCOPED_TRACE("4");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_commandNull, 1, 1, 1, 1, 0);
+
+  TeardownScheduler();
+}

wpilibcIntegrationTests/src/FRCUserProgram/cpp/command/MockCommand.cpp

@@ -9,6 +9,10 @@
 
 using namespace frc;
 
+MockCommand::MockCommand(Subsystem* subsys) : MockCommand() {
+  Requires(subsys);
+}
+
 MockCommand::MockCommand() {
   m_initializeCount = 0;
   m_executeCount = 0;
@@ -36,3 +40,12 @@ bool MockCommand::IsFinished() {
 void MockCommand::End() { ++m_endCount; }
 
 void MockCommand::Interrupted() { ++m_interruptedCount; }
+
+void MockCommand::ResetCounters() {
+  m_initializeCount = 0;
+  m_executeCount = 0;
+  m_isFinishedCount = 0;
+  m_hasFinished = false;
+  m_endCount = 0;
+  m_interruptedCount = 0;
+}

wpilibcIntegrationTests/src/FRCUserProgram/cpp/command/MockConditionalCommand.cpp

@@ -11,10 +11,47 @@ using namespace frc;
 
 MockConditionalCommand::MockConditionalCommand(MockCommand* onTrue,
                                                MockCommand* onFalse)
-    : ConditionalCommand(onTrue, onFalse) {}
+    : ConditionalCommand(onTrue, onFalse) {
+  m_initializeCount = 0;
+  m_executeCount = 0;
+  m_isFinishedCount = 0;
+  m_endCount = 0;
+  m_interruptedCount = 0;
+}
 
 void MockConditionalCommand::SetCondition(bool condition) {
   m_condition = condition;
 }
 
 bool MockConditionalCommand::Condition() { return m_condition; }
+
+bool MockConditionalCommand::HasInitialized() {
+  return GetInitializeCount() > 0;
+}
+
+bool MockConditionalCommand::HasEnd() { return GetEndCount() > 0; }
+
+bool MockConditionalCommand::HasInterrupted() {
+  return GetInterruptedCount() > 0;
+}
+
+void MockConditionalCommand::Initialize() { ++m_initializeCount; }
+
+void MockConditionalCommand::Execute() { ++m_executeCount; }
+
+bool MockConditionalCommand::IsFinished() {
+  ++m_isFinishedCount;
+  return ConditionalCommand::IsFinished();
+}
+
+void MockConditionalCommand::End() { ++m_endCount; }
+
+void MockConditionalCommand::Interrupted() { ++m_interruptedCount; }
+
+void MockConditionalCommand::ResetCounters() {
+  m_initializeCount = 0;
+  m_executeCount = 0;
+  m_isFinishedCount = 0;
+  m_endCount = 0;
+  m_interruptedCount = 0;
+}

wpilibcIntegrationTests/src/FRCUserProgram/headers/command/MockCommand.h

@@ -13,6 +13,7 @@ namespace frc {
 
 class MockCommand : public Command {
  public:
+  explicit MockCommand(Subsystem*);
   MockCommand();
   int32_t GetInitializeCount() { return m_initializeCount; }
   bool HasInitialized();
@@ -26,6 +27,7 @@ class MockCommand : public Command {
 
   int32_t GetInterruptedCount() { return m_interruptedCount; }
   bool HasInterrupted();
+  void ResetCounters();
 
  protected:
   void Initialize() override;

wpilibcIntegrationTests/src/FRCUserProgram/headers/command/MockConditionalCommand.h

@@ -16,12 +16,33 @@ class MockConditionalCommand : public ConditionalCommand {
  public:
   MockConditionalCommand(MockCommand* onTrue, MockCommand* onFalse);
   void SetCondition(bool condition);
+  int32_t GetInitializeCount() { return m_initializeCount; }
+  bool HasInitialized();
+
+  int32_t GetExecuteCount() { return m_executeCount; }
+  int32_t GetIsFinishedCount() { return m_isFinishedCount; }
+  int32_t GetEndCount() { return m_endCount; }
+  bool HasEnd();
+
+  int32_t GetInterruptedCount() { return m_interruptedCount; }
+  bool HasInterrupted();
+  void ResetCounters();
 
  protected:
   bool Condition() override;
+  void Initialize() override;
+  void Execute() override;
+  bool IsFinished() override;
+  void End() override;
+  void Interrupted() override;
 
  private:
   bool m_condition = false;
+  int32_t m_initializeCount;
+  int32_t m_executeCount;
+  int32_t m_isFinishedCount;
+  int32_t m_endCount;
+  int32_t m_interruptedCount;
 };
 
 }  // namespace frc

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

@@ -86,6 +86,11 @@ public abstract class Command extends SendableBase implements Sendable {
    */
   private boolean m_runWhenDisabled = false;
 
+  /**
+   * Whether or not this command has completed running.
+   */
+  private boolean m_completed = false;
+
   /**
    * The {@link CommandGroup} this is in.
    */
@@ -208,6 +213,7 @@ public abstract class Command extends SendableBase implements Sendable {
     m_initialized = false;
     m_canceled = false;
     m_running = false;
+    m_completed = true;
   }
 
   /**
@@ -404,6 +410,7 @@ public abstract class Command extends SendableBase implements Sendable {
           "Can not start a command that is a part of a command group");
     }
     Scheduler.getInstance().add(this);
+    m_completed = false;
   }
 
   /**
@@ -467,6 +474,15 @@ public abstract class Command extends SendableBase implements Sendable {
     return m_canceled;
   }
 
+  /**
+   * Whether or not this command has completed running.
+   *
+   * @return whether or not this command has completed running.
+   */
+  public synchronized boolean isCompleted() {
+    return m_completed;
+  }
+
   /**
    * Returns whether or not this command can be interrupted.
    *

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

@@ -143,6 +143,7 @@ public abstract class ConditionalCommand extends Command {
 
       m_chosenCommand.start();
     }
+    super._initialize();
   }
 
   @Override
@@ -156,16 +157,19 @@ public abstract class ConditionalCommand extends Command {
 
   @Override
   protected boolean isFinished() {
-    return m_chosenCommand != null && m_chosenCommand.isRunning()
-        && m_chosenCommand.isFinished();
+    if (m_chosenCommand != null) {
+      return m_chosenCommand.isCompleted();
+    } else {
+      return true;
+    }
   }
 
   @Override
-  protected void interrupted() {
+  protected void _interrupted() {
     if (m_chosenCommand != null && m_chosenCommand.isRunning()) {
       m_chosenCommand.cancel();
     }
 
-    super.interrupted();
+    super._interrupted();
   }
 }

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

@@ -7,22 +7,38 @@
 
 package edu.wpi.first.wpilibj.command;
 
+import static org.junit.Assert.assertEquals;
 import static org.junit.Assert.assertTrue;
 
 import org.junit.Before;
+//import org.junit.Ignore;
 import org.junit.Test;
 
 public class ConditionalCommandTest extends AbstractCommandTest {
   MockConditionalCommand m_command;
+  MockConditionalCommand m_commandNull;
   MockCommand m_onTrue;
   MockCommand m_onFalse;
+  MockSubsystem m_subsys;
   Boolean m_condition;
 
   @Before
   public void initCommands() {
-    m_onTrue = new MockCommand();
-    m_onFalse = new MockCommand();
+    m_subsys = new MockSubsystem();
+    m_onTrue = new MockCommand(m_subsys);
+    m_onFalse = new MockCommand(m_subsys);
     m_command = new MockConditionalCommand(m_onTrue, m_onFalse);
+    m_commandNull = new MockConditionalCommand(m_onTrue, null);
+  }
+
+  protected void assertConditionalCommandState(MockConditionalCommand command, int initialize,
+                                               int execute, int isFinished, int end,
+                                               int interrupted) {
+    assertEquals(initialize, command.getInitializeCount());
+    assertEquals(execute, command.getExecuteCount());
+    assertEquals(isFinished, command.getIsFinishedCount());
+    assertEquals(end, command.getEndCount());
+    assertEquals(interrupted, command.getInterruptedCount());
   }
 
   @Test
@@ -31,14 +47,33 @@ public class ConditionalCommandTest extends AbstractCommandTest {
 
     Scheduler.getInstance().add(m_command);
     assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
     Scheduler.getInstance().run();  // init command and select m_onTrue
     assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
     Scheduler.getInstance().run();  // init m_onTrue
     assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 1, 1, 1, 0, 0);
     Scheduler.getInstance().run();
-    assertCommandState(m_onTrue, 1, 1, 2, 0, 0);
+    assertCommandState(m_onTrue, 1, 1, 1, 0, 0);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 1, 2, 2, 0, 0);
     Scheduler.getInstance().run();
-    assertCommandState(m_onTrue, 1, 2, 4, 0, 0);
+    assertCommandState(m_onTrue, 1, 2, 2, 0, 0);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 1, 3, 3, 0, 0);
+    m_onTrue.setHasFinished(true);
+    Scheduler.getInstance().run();
+    assertCommandState(m_onTrue, 1, 3, 3, 1, 0);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 1, 4, 4, 0, 0);
+    Scheduler.getInstance().run();
+    assertCommandState(m_onTrue, 1, 3, 3, 1, 0);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 1, 5, 5, 1, 0);
 
     assertTrue("Did not initialize the true command", m_onTrue.getInitializeCount() > 0);
     assertTrue("Initialized the false command", m_onFalse.getInitializeCount() == 0);
@@ -50,16 +85,261 @@ public class ConditionalCommandTest extends AbstractCommandTest {
 
     Scheduler.getInstance().add(m_command);
     assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
     Scheduler.getInstance().run();  // init command and select m_onFalse
     assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
     Scheduler.getInstance().run();  // init m_onFalse
     assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 1, 1, 1, 0, 0);
     Scheduler.getInstance().run();
-    assertCommandState(m_onFalse, 1, 1, 2, 0, 0);
+    assertCommandState(m_onFalse, 1, 1, 1, 0, 0);
+    assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 1, 2, 2, 0, 0);
     Scheduler.getInstance().run();
-    assertCommandState(m_onFalse, 1, 2, 4, 0, 0);
+    assertCommandState(m_onFalse, 1, 2, 2, 0, 0);
+    assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 1, 3, 3, 0, 0);
+    m_onFalse.setHasFinished(true);
+    Scheduler.getInstance().run();
+    assertCommandState(m_onFalse, 1, 3, 3, 1, 0);
+    assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 1, 4, 4, 0, 0);
+    Scheduler.getInstance().run();
+    assertCommandState(m_onFalse, 1, 3, 3, 1, 0);
+    assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 1, 5, 5, 1, 0);
 
     assertTrue("Did not initialize the false command", m_onFalse.getInitializeCount() > 0);
     assertTrue("Initialized the true command", m_onTrue.getInitializeCount() == 0);
   }
+
+  @Test
+  public void testCancelSubCommand() {
+    m_command.setCondition(true);
+
+    Scheduler.getInstance().add(m_command);
+    assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
+    Scheduler.getInstance().run();  // init command and select m_onTrue
+    assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
+    Scheduler.getInstance().run();  // init m_onTrue
+    assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 1, 1, 1, 0, 0);
+    Scheduler.getInstance().run();
+    assertCommandState(m_onTrue, 1, 1, 1, 0, 0);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 1, 2, 2, 0, 0);
+    Scheduler.getInstance().run();
+    assertCommandState(m_onTrue, 1, 2, 2, 0, 0);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 1, 3, 3, 0, 0);
+    m_onTrue.cancel();
+    Scheduler.getInstance().run();
+    assertCommandState(m_onTrue, 1, 2, 2, 0, 1);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 1, 4, 4, 0, 0);
+    Scheduler.getInstance().run();
+    assertCommandState(m_onTrue, 1, 2, 2, 0, 1);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 1, 5, 5, 1, 0);
+    Scheduler.getInstance().run();
+    assertCommandState(m_onTrue, 1, 2, 2, 0, 1);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 1, 5, 5, 1, 0);
+  }
+
+  @Test
+  public void testCancelRequires() {
+    m_command.setCondition(true);
+
+    Scheduler.getInstance().add(m_command);
+    assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
+    Scheduler.getInstance().run();  // init command and select m_onTrue
+    assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
+    Scheduler.getInstance().run();  // init m_onTrue
+    assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 1, 1, 1, 0, 0);
+    Scheduler.getInstance().run();
+    assertCommandState(m_onTrue, 1, 1, 1, 0, 0);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 1, 2, 2, 0, 0);
+    Scheduler.getInstance().run();
+    assertCommandState(m_onTrue, 1, 2, 2, 0, 0);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 1, 3, 3, 0, 0);
+    m_onFalse.start();
+    Scheduler.getInstance().run();
+    assertCommandState(m_onTrue, 1, 3, 3, 0, 0);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 1, 4, 4, 0, 1);
+    Scheduler.getInstance().run();
+    assertCommandState(m_onTrue, 1, 3, 3, 0, 1);
+    assertCommandState(m_onFalse, 1, 1, 1, 0, 0);
+    assertConditionalCommandState(m_command, 1, 4, 4, 0, 1);
+  }
+
+  @Test
+  public void testCancelCondCommand() {
+    m_command.setCondition(true);
+
+    Scheduler.getInstance().add(m_command);
+    assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
+    Scheduler.getInstance().run();  // init command and select m_onTrue
+    assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
+    Scheduler.getInstance().run();  // init m_onTrue
+    assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 1, 1, 1, 0, 0);
+    Scheduler.getInstance().run();
+    assertCommandState(m_onTrue, 1, 1, 1, 0, 0);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 1, 2, 2, 0, 0);
+    Scheduler.getInstance().run();
+    assertCommandState(m_onTrue, 1, 2, 2, 0, 0);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 1, 3, 3, 0, 0);
+    m_command.cancel();
+    Scheduler.getInstance().run();
+    assertCommandState(m_onTrue, 1, 2, 2, 0, 1);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 1, 3, 3, 0, 1);
+    Scheduler.getInstance().run();
+    assertCommandState(m_onTrue, 1, 2, 2, 0, 1);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 1, 3, 3, 0, 1);
+  }
+
+  @Test
+  public void testOnTrueTwice() {
+    m_command.setCondition(true);
+
+    Scheduler.getInstance().add(m_command);
+    assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
+    Scheduler.getInstance().run();  // init command and select m_onTrue
+    assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
+    Scheduler.getInstance().run();  // init m_onTrue
+    assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 1, 1, 1, 0, 0);
+    Scheduler.getInstance().run();
+    assertCommandState(m_onTrue, 1, 1, 1, 0, 0);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 1, 2, 2, 0, 0);
+    Scheduler.getInstance().run();
+    assertCommandState(m_onTrue, 1, 2, 2, 0, 0);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 1, 3, 3, 0, 0);
+    m_onTrue.setHasFinished(true);
+    Scheduler.getInstance().run();
+    assertCommandState(m_onTrue, 1, 3, 3, 1, 0);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 1, 4, 4, 0, 0);
+    Scheduler.getInstance().run();
+    assertCommandState(m_onTrue, 1, 3, 3, 1, 0);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 1, 5, 5, 1, 0);
+
+    m_onTrue.resetCounters();
+    m_command.resetCounters();
+    m_command.setCondition(true);
+    Scheduler.getInstance().add(m_command);
+    assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
+    Scheduler.getInstance().run();  // init command and select m_onTrue
+    assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
+    Scheduler.getInstance().run();  // init m_onTrue
+    assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 1, 1, 1, 0, 0);
+    Scheduler.getInstance().run();
+    assertCommandState(m_onTrue, 1, 1, 1, 0, 0);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 1, 2, 2, 0, 0);
+    Scheduler.getInstance().run();
+    assertCommandState(m_onTrue, 1, 2, 2, 0, 0);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 1, 3, 3, 0, 0);
+    m_onTrue.setHasFinished(true);
+    Scheduler.getInstance().run();
+    assertCommandState(m_onTrue, 1, 3, 3, 1, 0);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 1, 4, 4, 0, 0);
+    Scheduler.getInstance().run();
+    assertCommandState(m_onTrue, 1, 3, 3, 1, 0);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 1, 5, 5, 1, 0);
+  }
+
+  @Test
+  public void testOnTrueInstant() {
+    m_command.setCondition(true);
+    m_onTrue.setHasFinished(true);
+
+    Scheduler.getInstance().add(m_command);
+    assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
+    Scheduler.getInstance().run();  // init command and select m_onTrue
+    assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
+    Scheduler.getInstance().run();  // init m_onTrue
+    assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 1, 1, 1, 0, 0);
+    Scheduler.getInstance().run();
+    assertCommandState(m_onTrue, 1, 1, 1, 1, 0);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 1, 2, 2, 0, 0);
+    Scheduler.getInstance().run();
+    assertCommandState(m_onTrue, 1, 1, 1, 1, 0);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 1, 3, 3, 1, 0);
+    Scheduler.getInstance().run();
+    assertCommandState(m_onTrue, 1, 1, 1, 1, 0);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 1, 3, 3, 1, 0);
+  }
+
+  @Test
+  public void testOnFalseNull() {
+    m_commandNull.setCondition(false);
+
+    Scheduler.getInstance().add(m_commandNull);
+    assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_commandNull, 0, 0, 0, 0, 0);
+    Scheduler.getInstance().run();  // init command and select m_onFalse
+    assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_commandNull, 0, 0, 0, 0, 0);
+    Scheduler.getInstance().run();  // init m_onFalse
+    assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_commandNull, 1, 1, 1, 1, 0);
+    Scheduler.getInstance().run();
+    assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_commandNull, 1, 1, 1, 1, 0);
+  }
 }

wpilibj/src/test/java/edu/wpi/first/wpilibj/command/MockCommand.java

@@ -8,7 +8,7 @@
 package edu.wpi.first.wpilibj.command;
 
 /**
- * A class to simulate a simple command The command keeps track of how many times each method was
+ * A class to simulate a simple command. The command keeps track of how many times each method was
  * called.
  */
 public class MockCommand extends Command {
@@ -19,6 +19,15 @@ public class MockCommand extends Command {
   private int m_endCount = 0;
   private int m_interruptedCount = 0;
 
+  public MockCommand(Subsystem subsys) {
+    super();
+    requires(subsys);
+  }
+
+  public MockCommand() {
+    super();
+  }
+
   protected void initialize() {
     ++m_initializeCount;
   }
@@ -115,4 +124,16 @@ public class MockCommand extends Command {
     return getInterruptedCount() > 0;
   }
 
+  /**
+   * Reset internal counters.
+   */
+  public void resetCounters() {
+    m_initializeCount = 0;
+    m_executeCount = 0;
+    m_isFinishedCount = 0;
+    m_hasFinished = false;
+    m_endCount = 0;
+    m_interruptedCount = 0;
+  }
+
 }

wpilibj/src/test/java/edu/wpi/first/wpilibj/command/MockConditionalCommand.java

@@ -9,6 +9,11 @@ package edu.wpi.first.wpilibj.command;
 
 public class MockConditionalCommand extends ConditionalCommand {
   private boolean m_condition = false;
+  private int m_initializeCount = 0;
+  private int m_executeCount = 0;
+  private int m_isFinishedCount = 0;
+  private int m_endCount = 0;
+  private int m_interruptedCount = 0;
 
   public MockConditionalCommand(MockCommand onTrue, MockCommand onFalse) {
     super(onTrue, onFalse);
@@ -22,4 +27,94 @@ public class MockConditionalCommand extends ConditionalCommand {
   public void setCondition(boolean condition) {
     this.m_condition = condition;
   }
+
+  protected void initialize() {
+    ++m_initializeCount;
+  }
+
+  protected void execute() {
+    ++m_executeCount;
+  }
+
+  protected boolean isFinished() {
+    ++m_isFinishedCount;
+    return super.isFinished();
+  }
+
+  protected void end() {
+    ++m_endCount;
+  }
+
+  protected void interrupted() {
+    ++m_interruptedCount;
+  }
+
+
+  /**
+   * How many times the initialize method has been called.
+   */
+  public int getInitializeCount() {
+    return m_initializeCount;
+  }
+
+  /**
+   * If the initialize method has been called at least once.
+   */
+  public boolean hasInitialized() {
+    return getInitializeCount() > 0;
+  }
+
+  /**
+   * How many time the execute method has been called.
+   */
+  public int getExecuteCount() {
+    return m_executeCount;
+  }
+
+  /**
+   * How many times the isFinished method has been called.
+   */
+  public int getIsFinishedCount() {
+    return m_isFinishedCount;
+  }
+
+  /**
+   * How many times the end method has been called.
+   */
+  public int getEndCount() {
+    return m_endCount;
+  }
+
+  /**
+   * If the end method has been called at least once.
+   */
+  public boolean hasEnd() {
+    return getEndCount() > 0;
+  }
+
+  /**
+   * How many times the interrupted method has been called.
+   */
+  public int getInterruptedCount() {
+    return m_interruptedCount;
+  }
+
+  /**
+   * If the interrupted method has been called at least once.
+   */
+  public boolean hasInterrupted() {
+    return getInterruptedCount() > 0;
+  }
+
+  /**
+   * Reset internal counters.
+   */
+  public void resetCounters() {
+    m_condition = false;
+    m_initializeCount = 0;
+    m_executeCount = 0;
+    m_isFinishedCount = 0;
+    m_endCount = 0;
+    m_interruptedCount = 0;
+  }
 }

wpilibj/src/test/java/edu/wpi/first/wpilibj/command/MockSubsystem.java

@@ -0,0 +1,15 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.command;
+
+/**
+ * A class to simulate a simple subsystem.
+ */
+public class MockSubsystem extends Subsystem {
+  protected void initDefaultCommand() {}
+}