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

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java

@@ -302,6 +302,18 @@ public interface Command {
     return new ProxyScheduleCommand(this);
   }
 
+  /**
+   * 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
+   */
+  default ConditionalCommand unless(BooleanSupplier condition) {
+    return new ConditionalCommand(new InstantCommand(), this, condition);
+  }
+
   /**
    * Schedules this command.
    *

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.
    *

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandDecoratorTest.java

@@ -9,6 +9,7 @@ import static org.junit.jupiter.api.Assertions.assertTrue;
 
 import edu.wpi.first.hal.HAL;
 import edu.wpi.first.wpilibj.simulation.SimHooks;
+import java.util.concurrent.atomic.AtomicBoolean;
 import org.junit.jupiter.api.Test;
 import org.junit.jupiter.api.parallel.ResourceLock;
 
@@ -182,4 +183,24 @@ class CommandDecoratorTest extends CommandTestBase {
       assertTrue(scheduler.isScheduled(perpetual));
     }
   }
+
+  @Test
+  void unlessTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicBoolean unlessCondition = new AtomicBoolean(true);
+      AtomicBoolean hasRunCondition = new AtomicBoolean(false);
+
+      Command command =
+          new InstantCommand(() -> hasRunCondition.set(true)).unless(unlessCondition::get);
+
+      scheduler.schedule(command);
+      scheduler.run();
+      assertFalse(hasRunCondition.get());
+
+      unlessCondition.set(false);
+      scheduler.schedule(command);
+      scheduler.run();
+      assertTrue(hasRunCondition.get());
+    }
+  }
 }

wpilibNewCommands/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp

@@ -5,6 +5,7 @@
 #include <frc/simulation/SimHooks.h>
 
 #include "CommandTestBase.h"
+#include "frc2/command/ConditionalCommand.h"
 #include "frc2/command/InstantCommand.h"
 #include "frc2/command/ParallelRaceGroup.h"
 #include "frc2/command/PerpetualCommand.h"
@@ -101,3 +102,24 @@ TEST_F(CommandDecoratorTest, Perpetually) {
 
   EXPECT_TRUE(scheduler.IsScheduled(&command));
 }
+
+TEST_F(CommandDecoratorTest, Unless) {
+  CommandScheduler scheduler = GetScheduler();
+
+  bool hasRun = false;
+  bool unlessBool = true;
+
+  auto command =
+      InstantCommand([&hasRun] { hasRun = true; }, {}).Unless([&unlessBool] {
+        return unlessBool;
+      });
+
+  scheduler.Schedule(&command);
+  scheduler.Run();
+  EXPECT_FALSE(hasRun);
+
+  unlessBool = false;
+  scheduler.Schedule(&command);
+  scheduler.Run();
+  EXPECT_TRUE(hasRun);
+}