[commands] Add unless() decorator (#4244)

wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp

@@ -5,6 +5,7 @@
 #include "frc2/command/Command.h"
 
 #include "frc2/command/CommandScheduler.h"
+#include "frc2/command/ConditionalCommand.h"
 #include "frc2/command/EndlessCommand.h"
 #include "frc2/command/InstantCommand.h"
 #include "frc2/command/ParallelCommandGroup.h"
@@ -101,6 +102,12 @@ ProxyScheduleCommand Command::AsProxy() {
   return ProxyScheduleCommand(this);
 }
 
+ConditionalCommand Command::Unless(std::function<bool()> condition) && {
+  return ConditionalCommand(std::make_unique<InstantCommand>(),
+                            std::move(*this).TransferOwnership(),
+                            std::move(condition));
+}
+
 void Command::Schedule(bool interruptible) {
   CommandScheduler::GetInstance().Schedule(interruptible, this);
 }

wpilibNewCommands/src/main/native/include/frc2/command/Command.h

@@ -32,6 +32,7 @@ class SequentialCommandGroup;
 class PerpetualCommand;
 class ProxyScheduleCommand;
 class RepeatCommand;
+class ConditionalCommand;
 
 /**
  * A state machine representing a complete action to be performed by the robot.
@@ -39,8 +40,9 @@ class RepeatCommand;
  * CommandGroups to allow users to build complicated multi-step actions without
  * the need to roll the state machine logic themselves.
  *
- * <p>Commands are run synchronously from the main robot loop; no multithreading
- * is used, unless specified explicitly from the command implementation.
+ * <p>Commands are run synchronously from the main robot loop; no
+ * multithreading is used, unless specified explicitly from the command
+ * implementation.
  *
  * <p>Note: ALWAYS create a subclass by extending CommandHelper<Base, Subclass>,
  * or decorators will not function!
@@ -216,6 +218,17 @@ class Command {
    */
   virtual ProxyScheduleCommand AsProxy();
 
+  /**
+   * Decorates this command to only run if this condition is not met. If the
+   * command is already running and the condition changes to true, the command
+   * will not stop running. The requirements of this command will be kept for
+   * the new conditonal command.
+   *
+   * @param condition the condition that will prevent the command from running
+   * @return the decorated command
+   */
+  virtual ConditionalCommand Unless(std::function<bool()> condition) &&;
+
   /**
    * Schedules this command.
    *