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