Add wait for duration, match time, and condition, as well as ifThenElse

Author: James Hagborg

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

@@ -1,7 +1,13 @@
 package org.usfirst.frc.team69.util;
 
+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.InstantCommand;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+import edu.wpi.first.wpilibj.command.WaitUntilCommand;
 
 /**
  * A builder class to create commands in autonomous.  Currently, it wraps a
@@ -10,7 +16,7 @@
  * To build a command, add child commands using {@link #sequential(Command)}
  * and {@link #parallel(Command)}, then get a command using {@link #build()}.
  * 
- * @author James
+ * @author James Hagborg
  *
  */
 public class CommandBuilder {
@@ -86,6 +92,79 @@ public CommandBuilder parallel(Command command, double timeout) {
         return this;
     }
 
+    /**
+     * Wait for a given duration.
+     * 
+     * @param seconds The time to wait, in seconds
+     * @return This CommandBuilder object
+     */
+    public CommandBuilder waitForDuration(double seconds) {
+        m_cmdGroup.addSequential(new WaitCommand(seconds));
+        return this;
+    }
+    
+    /**
+     * Wait until a given match time.
+     * 
+     * @param seconds The match time to wait for, in seconds from the start
+     * @return This CommandBuilder object
+     */
+    public CommandBuilder waitForMatchTime(double seconds) {
+        m_cmdGroup.addSequential(new WaitUntilCommand(seconds));
+        return this;
+    }
+    
+    /**
+     * Wait until an arbitrary condition is met.
+     * 
+     * @param condition The condition to wait for.
+     * @return This CommandBuilder object
+     */
+    public CommandBuilder waitForCondition(BooleanSupplier condition) {
+        m_cmdGroup.addSequential(QuickCommand.waitFor(condition));
+        return this;
+    }
+    
+    /**
+     * Wait until an arbitrary condition is met, or until a timeout expires.
+     * 
+     * @param condition The condition to wait for
+     * @param timeout The timeout in seconds
+     * @return This CommandBuilder object
+     */
+    public CommandBuilder waitForCondition(BooleanSupplier condition, double timeout) {
+        m_cmdGroup.addSequential(QuickCommand.waitFor(condition), timeout);
+        return this;
+    }
+    
+    /**
+     * Run a command if and only if a condition is met at runtime.
+     * 
+     * @param condition The condition to check
+     * @param ifTrue The command to run if the condition is true
+     * @return This CommandBuilder object
+     */
+    public CommandBuilder ifThen(BooleanSupplier condition, Command ifTrue) {
+        return ifThenElse(condition, ifTrue, new InstantCommand());
+    }
+    
+    /**
+     * Run one of two commands, depending on if a condition is true.
+     * 
+     * @param condition The condition to check
+     * @param ifTrue The command to run if the condition is true
+     * @param ifFalse The command to run if the condition is false
+     * @return This CommandBuilder object
+     */
+    public CommandBuilder ifThenElse(BooleanSupplier condition, Command ifTrue, Command ifFalse) {
+        m_cmdGroup.addSequential(new ConditionalCommand(ifTrue, ifFalse) {
+            @Override protected boolean condition() {
+                return condition.getAsBoolean();
+            }
+        });
+        return this;
+    }
+    
     /**
      * Build a command.  Note that for simplicity, calling this method 
      * multiple times will return the same {@link Command} object.  This