Add Lambda support to InstantCommand (#1262)

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

@@ -14,11 +14,14 @@ package edu.wpi.first.wpilibj.command;
  * {@link Command isFinished}.
  */
 public class InstantCommand extends Command {
+  private Runnable m_func;
+
   public InstantCommand() {
   }
 
   /**
    * Creates a new {@link InstantCommand InstantCommand} with the given name.
+   *
    * @param name the name for this command
    */
   public InstantCommand(String name) {
@@ -27,6 +30,7 @@ public class InstantCommand extends Command {
 
   /**
    * Creates a new {@link InstantCommand InstantCommand} with the given requirement.
+   *
    * @param subsystem the subsystem this command requires
    */
   public InstantCommand(Subsystem subsystem) {
@@ -35,6 +39,7 @@ public class InstantCommand extends Command {
 
   /**
    * Creates a new {@link InstantCommand InstantCommand} with the given name and requirement.
+   *
    * @param name      the name for this command
    * @param subsystem the subsystem this command requires
    */
@@ -42,8 +47,64 @@ public class InstantCommand extends Command {
     super(name, subsystem);
   }
 
+  /**
+   * Creates a new {@link InstantCommand InstantCommand}.
+   *
+   * @param func the function to run when {@link Command#initialize() initialize()} is run
+   */
+  public InstantCommand(Runnable func) {
+    m_func = func;
+  }
+
+  /**
+   * Creates a new {@link InstantCommand InstantCommand}.
+   *
+   * @param name the name for this command
+   * @param func the function to run when {@link Command#initialize() initialize()} is run
+   */
+  public InstantCommand(String name, Runnable func) {
+    super(name);
+    m_func = func;
+  }
+
+  /**
+   * Creates a new {@link InstantCommand InstantCommand}.
+   *
+   * @param requirement the subsystem this command requires
+   * @param func        the function to run when {@link Command#initialize() initialize()} is run
+   */
+  public InstantCommand(Subsystem requirement, Runnable func) {
+    super(requirement);
+    m_func = func;
+  }
+
+  /**
+   * Creates a new {@link InstantCommand InstantCommand}.
+   *
+   * @param name        the name for this command
+   * @param requirement the subsystem this command requires
+   * @param func        the function to run when {@link Command#initialize() initialize()} is run
+   */
+  public InstantCommand(String name, Subsystem requirement, Runnable func) {
+    super(name, requirement);
+    m_func = func;
+  }
+
   @Override
   protected boolean isFinished() {
     return true;
   }
+
+  /**
+   * Trigger the stored function.
+   *
+   * <p>Called just before this Command runs the first time.
+   */
+  @Override
+  protected void _initialize() {
+    super._initialize();
+    if (m_func != null) {
+      m_func.run();
+    }
+  }
 }