Fix cancel of inner commands in ConditionalCommands (#858)

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