Add release and releaseAll() methods for CommandBuilder

Author: James Hagborg

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

@@ -0,0 +1,26 @@
+package edu.wpi.first.wpilibj.command;
+
+import java.util.Enumeration;
+
+/**
+ * Helper class to get the requirements of a {@link Command}.  This is necessary
+ * because the getRequirements method is package-scope, but we want to use it
+ * anyway.
+ * 
+ * @author James Hagborg
+ *
+ */
+public class GetRequirements {
+    private GetRequirements() {}
+    
+    /**
+     * Get the requirements of a command
+     * @param cmd The command to use
+     * @return An enumeration of the requirements
+     */
+    @SuppressWarnings("rawtypes")
+    public static Enumeration getRequirements(Command cmd) {
+        return cmd.getRequirements();
+    }
+
+}

src/main/java/org/usfirst/frc/team69/util/CommandBuilder.java

@@ -1,11 +1,14 @@
 package org.usfirst.frc.team69.util;
 
+import java.util.Collections;
 import java.util.function.BooleanSupplier;
 
 import edu.wpi.first.wpilibj.command.Command;
 import edu.wpi.first.wpilibj.command.CommandGroup;
 import edu.wpi.first.wpilibj.command.ConditionalCommand;
+import edu.wpi.first.wpilibj.command.GetRequirements;
 import edu.wpi.first.wpilibj.command.InstantCommand;
+import edu.wpi.first.wpilibj.command.Subsystem;
 import edu.wpi.first.wpilibj.command.WaitCommand;
 import edu.wpi.first.wpilibj.command.WaitUntilCommand;
 
@@ -165,6 +168,39 @@ public CommandBuilder ifThenElse(BooleanSupplier condition, Command ifTrue, Comm
         return this;
     }
 
+    /**
+     * End any command currently running on the subsystem.  This is accomplished
+     * by running a command which ends instantly, which requires the given
+     * subsystem.  Afterwards, the default command will run, if there is one.
+     * 
+     * @param subsystem The subsystem to release
+     * @return This CommandBuilder object
+     */
+    public CommandBuilder release(Subsystem subsystem) {
+        m_cmdGroup.addSequential(QuickCommand.release(subsystem));
+        return this;
+    }
+    
+    /**
+     * End all commands currently running in parallel.  This is accomplished by
+     * starting an command which requires all the subsystems used so far,
+     * which ends instantly.  This will have the effect of starting the default
+     * commands for all these subsystems, if they have any.
+     * 
+     * @return This CommandBuilder object
+     */
+    @SuppressWarnings("unchecked")
+    public CommandBuilder releaseAll() {
+        m_cmdGroup.addSequential(new InstantCommand() {
+            {
+                for (Object req : Collections.list(GetRequirements.getRequirements(m_cmdGroup))) {
+                    requires((Subsystem) req);
+                }
+            }
+        });
+        return this;
+    }
+    
     /**
      * Build a command.  Note that for simplicity, calling this method 
      * multiple times will return the same {@link Command} object.  This