[cmd3] Add trigger multi press and continuous bindings (#8901)

`Trigger.multiPress(N, T)` creates a trigger that fires when there are at least N rising edges within the last T timespan, eg `multiPress(3, Seconds.of(1.25))` for 3 rising edges within the last 1.25 seconds. `Trigger.retryWhileTrue` and `retryWhileFalse` will continuously attempt to schedule a command while the binding condition is met. This is similar to `whileTrue` and `whileFalse`, but will reschedule the command if it ends or is canceled while the binding condition is still met, rather than requiring a new rising edge to reschedule it.
2026-06-19 00:41:43 +00:00 · 2026-05-23 19:51:45 -04:00
parent f217dfe747
commit 94fda36e04
2 changed files with 303 additions and 2 deletions
--- a/commandsv3/src/main/java/org/wpilib/command3/Trigger.java
+++ b/commandsv3/src/main/java/org/wpilib/command3/Trigger.java
@@ -7,13 +7,16 @@ package org.wpilib.command3;
 import static org.wpilib.units.Units.Seconds;
 import static org.wpilib.util.ErrorMessages.requireNonNullParam;

+import java.util.ArrayDeque;
 import java.util.ArrayList;
+import java.util.Deque;
 import java.util.EnumMap;
 import java.util.List;
 import java.util.Map;
 import java.util.function.BooleanSupplier;
 import org.wpilib.event.EventLoop;
 import org.wpilib.math.filter.Debouncer;
+import org.wpilib.system.Timer;
 import org.wpilib.units.measure.Time;

 /**
@@ -140,7 +143,8 @@ public class Trigger implements BooleanSupplier {
   * Starts the given command when the condition changes to `true` and cancels it when the condition
   * changes to `false`.
   *
-   * <p>Doesn't re-start the command if it ends while the condition is still `true`.
+   * <p>Unlike {@link #retryWhileTrue(Command)}, this does <strong>not</strong> restart the command
+   * if it ends while the condition is still `true`.
   *
   * @param command the command to start
   * @return this trigger, so calls can be chained
@@ -155,7 +159,8 @@ public class Trigger implements BooleanSupplier {
   * Starts the given command when the condition changes to `false` and cancels it when the
   * condition changes to `true`.
   *
-   * <p>Doesn't re-start the command if it ends while the condition is still `false`.
+   * <p>Unlike {@link #retryWhileFalse(Command)}, this does <strong>not</strong> restart the command
+   * if it ends while the condition is still `false`.
   *
   * @param command the command to start
   * @return this trigger, so calls can be chained
@@ -166,6 +171,40 @@ public class Trigger implements BooleanSupplier {
    return this;
  }

+  /**
+   * Starts the given command when the condition changes to `true` and cancels it when the condition
+   * changes to `false`.
+   *
+   * <p>Unlike {@link #whileTrue(Command)}, the command is restarted if it ends while the condition
+   * is still `true`. If the command stopped because it was interrupted, restarting it will
+   * immediately interrupt the would-be interrupting command (if they have the same priority).
+   *
+   * @param command the command to start
+   * @return this trigger, so calls can be chained
+   */
+  public Trigger retryWhileTrue(Command command) {
+    requireNonNullParam(command, "command", "retryWhileTrue");
+    addBinding(BindingType.CONTINUOUSLY_SCHEDULE_WHILE_HIGH, command);
+    return this;
+  }
+
+  /**
+   * Starts the given command when the condition changes to `false` and cancels it when the
+   * condition changes to `true`.
+   *
+   * <p>Unlike {@link #whileFalse(Command)}, the command is restarted if it ends while the condition
+   * is still `false`. If the command stopped because it was interrupted, restarting it will
+   * immediately interrupt the would-be interrupting command (if they have the same priority).
+   *
+   * @param command the command to start
+   * @return this trigger, so calls can be chained
+   */
+  public Trigger retryWhileFalse(Command command) {
+    requireNonNullParam(command, "command", "retryWhileFalse");
+    addBinding(BindingType.CONTINUOUSLY_SCHEDULE_WHILE_LOW, command);
+    return this;
+  }
+
  /**
   * Toggles a command when the condition changes from `false` to `true`.
   *
@@ -290,6 +329,63 @@ public class Trigger implements BooleanSupplier {
        m_scheduler, m_loop, () -> m_cachedSignal == Signal.LOW && m_previousSignal == Signal.HIGH);
  }

+  /**
+   * Creates a trigger that activates when this trigger has at least {@code pressCount} rising edges
+   * within the specified duration.
+   *
+   * @param pressCount The number of rising edges to require. If this is non-positive, the trigger
+   *     will always be active.
+   * @param duration The duration within which the rising edges must occur. If this is non-positive,
+   *     the trigger will never activate.
+   * @return A trigger that activates on multiple presses
+   */
+  public Trigger multiPress(int pressCount, Time duration) {
+    requireNonNullParam(duration, "duration", "multiTap");
+
+    // Short circuits to avoid unnecessary state tracking and object allocations
+    if (duration.baseUnitMagnitude() <= 0) {
+      // A nonpositive window size can never be met
+      return new Trigger(m_scheduler, m_loop, () -> false);
+    } else if (pressCount <= 0) {
+      // A nonpositive press count is always met
+      return new Trigger(m_scheduler, m_loop, () -> true);
+    }
+
+    final double durationSeconds = duration.in(Seconds);
+
+    return new Trigger(
+        m_scheduler,
+        m_loop,
+        new BooleanSupplier() {
+          // Note: unlike EdgeCounterFilter, this implementation tracks timestamps directly
+          // and remains high even if the signal is low, just as long as the number of rising edges
+          // meets the criteria. EdgeCounterFilter is only high when the requisite number of edges
+          // have been seen _and_ the signal is high.
+          private final Deque<Double> m_timestamps = new ArrayDeque<>();
+          private boolean m_risingEdgeOccurred = false;
+
+          @Override
+          public boolean getAsBoolean() {
+            if (m_cachedSignal == Signal.HIGH && m_previousSignal != Signal.HIGH) {
+              if (!m_risingEdgeOccurred) {
+                m_timestamps.addLast(Timer.getTimestamp());
+                m_risingEdgeOccurred = true;
+              }
+            } else if (m_cachedSignal != Signal.HIGH) {
+              m_risingEdgeOccurred = false;
+            }
+
+            double currentTime = Timer.getTimestamp();
+            while (!m_timestamps.isEmpty()
+                && currentTime - m_timestamps.peekFirst() > durationSeconds + 1e-9) {
+              m_timestamps.removeFirst();
+            }
+
+            return m_timestamps.size() >= pressCount;
+          }
+        });
+  }
+
  private void poll() {
    // Clear bindings that no longer need to run
    // This should always be checked, regardless of signal change, since bindings may be scoped
@@ -299,6 +395,13 @@ public class Trigger implements BooleanSupplier {
    m_previousSignal = m_cachedSignal;
    m_cachedSignal = readSignal();

+    // Always attempt to schedule bindings based on the current signal
+    if (m_cachedSignal == Signal.HIGH) {
+      scheduleBindings(BindingType.CONTINUOUSLY_SCHEDULE_WHILE_HIGH);
+    } else if (m_cachedSignal == Signal.LOW) {
+      scheduleBindings(BindingType.CONTINUOUSLY_SCHEDULE_WHILE_LOW);
+    }
+
    if (m_cachedSignal == m_previousSignal) {
      // No change in the signal. Nothing to do
      return;
@@ -309,6 +412,7 @@ public class Trigger implements BooleanSupplier {
      scheduleBindings(BindingType.SCHEDULE_ON_RISING_EDGE);
      scheduleBindings(BindingType.RUN_WHILE_HIGH);
      cancelBindings(BindingType.RUN_WHILE_LOW);
+      cancelBindings(BindingType.CONTINUOUSLY_SCHEDULE_WHILE_LOW);
      toggleBindings(BindingType.TOGGLE_ON_RISING_EDGE);
    }

@@ -317,6 +421,7 @@ public class Trigger implements BooleanSupplier {
      scheduleBindings(BindingType.SCHEDULE_ON_FALLING_EDGE);
      scheduleBindings(BindingType.RUN_WHILE_LOW);
      cancelBindings(BindingType.RUN_WHILE_HIGH);
+      cancelBindings(BindingType.CONTINUOUSLY_SCHEDULE_WHILE_HIGH);
      toggleBindings(BindingType.TOGGLE_ON_FALLING_EDGE);
    }
  }
--- a/commandsv3/src/test/java/org/wpilib/command3/TriggerTest.java
+++ b/commandsv3/src/test/java/org/wpilib/command3/TriggerTest.java
@@ -8,11 +8,14 @@ import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertFalse;
 import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.wpilib.units.Units.Seconds;

 import java.util.List;
 import java.util.concurrent.atomic.AtomicBoolean;
+import java.util.concurrent.atomic.AtomicLong;
 import java.util.function.BooleanSupplier;
 import org.junit.jupiter.api.Test;
+import org.wpilib.system.RobotController;

 class TriggerTest extends CommandTestBase {
  @Test
@@ -634,6 +637,199 @@ class TriggerTest extends CommandTestBase {
    // The trigger should have unbound itself during the last run() call.
  }

+  @Test
+  void multiPress() {
+    var currentTimeMicros = new AtomicLong(1000000); // Start at 1s
+    RobotController.setTimeSource(currentTimeMicros::get);
+
+    var signal = new AtomicBoolean(false);
+    var baseTrigger = new Trigger(m_scheduler, signal::get);
+    var multiPressTrigger = baseTrigger.multiPress(3, Seconds.of(1));
+
+    m_scheduler.run();
+    assertFalse(multiPressTrigger.getAsBoolean(), "Should not fire initially");
+
+    // First press at 1.1s
+    currentTimeMicros.set(1100000);
+    signal.set(true);
+    m_scheduler.run();
+    assertFalse(multiPressTrigger.getAsBoolean(), "Should not fire after 1 press");
+
+    signal.set(false);
+    m_scheduler.run();
+
+    // Second press at 1.2s
+    currentTimeMicros.set(1200000);
+    signal.set(true);
+    m_scheduler.run();
+    assertFalse(multiPressTrigger.getAsBoolean(), "Should not fire after 2 presses");
+
+    signal.set(false);
+    m_scheduler.run();
+
+    // Third press at 1.3s
+    currentTimeMicros.set(1300000);
+    signal.set(true);
+    m_scheduler.run();
+    assertTrue(multiPressTrigger.getAsBoolean(), "Should fire after 3 presses");
+
+    signal.set(false);
+    m_scheduler.run();
+
+    // Fourth press at 2.0s (First press at 1.1s should be NOT yet expired, so 1.1s, 1.2s, 1.3s,
+    // 2.0s ->
+    // 4 presses)
+    currentTimeMicros.set(2000000);
+    signal.set(true);
+    m_scheduler.run();
+    assertTrue(
+        multiPressTrigger.getAsBoolean(),
+        "Should still fire as there are 4 presses within last 1s");
+
+    signal.set(false);
+    m_scheduler.run();
+
+    // Wait until 2.2s. Press at 1.1s is expired (exactly 1.1s elapsed).
+    // Remaining: 1.2s, 1.3s, 2.0s -> 3 presses.
+    currentTimeMicros.set(2200000);
+    m_scheduler.run();
+    assertTrue(
+        multiPressTrigger.getAsBoolean(),
+        "Should still fire as there are 3 presses within last 1s");
+
+    // Wait until 2.4s. Presses at 1.2s and 1.3s are definitely expired. Only 2.0s remains.
+    currentTimeMicros.set(2400000);
+    m_scheduler.run();
+    assertFalse(multiPressTrigger.getAsBoolean(), "Should not fire after presses expire");
+  }
+
+  @Test
+  void multiPressZeroDuration() {
+    var trigger = new Trigger(m_scheduler, () -> true);
+    var zeroDuration = trigger.multiPress(1, Seconds.of(0));
+    m_scheduler.run();
+    assertFalse(zeroDuration.getAsBoolean(), "Zero duration multiPress should always be false");
+  }
+
+  @Test
+  void multiPressNegativeDuration() {
+    var trigger = new Trigger(m_scheduler, () -> true);
+    var negativeDuration = trigger.multiPress(1, Seconds.of(-1));
+    m_scheduler.run();
+    assertFalse(
+        negativeDuration.getAsBoolean(), "Negative duration multiPress should always be false");
+  }
+
+  @Test
+  void multiPressZeroPressCount() {
+    var trigger = new Trigger(m_scheduler, () -> true);
+    var zeroPressCount = trigger.multiPress(0, Seconds.of(1));
+    m_scheduler.run();
+    assertTrue(zeroPressCount.getAsBoolean(), "Zero press count multiPress should always be true");
+  }
+
+  @Test
+  void multiPressNegativePressCount() {
+    var trigger = new Trigger(m_scheduler, () -> true);
+    var negativePressCount = trigger.multiPress(-1, Seconds.of(1));
+    m_scheduler.run();
+    assertTrue(
+        negativePressCount.getAsBoolean(), "Negative press count multiPress should always be true");
+  }
+
+  @Test
+  void retryWhileTrueLongRunningCanceled() {
+    var signal = new AtomicBoolean(false);
+    var trigger = new Trigger(m_scheduler, signal::get);
+    var command = Command.noRequirements(Coroutine::park).named("Command");
+    trigger.retryWhileTrue(command);
+
+    signal.set(true);
+    m_scheduler.run();
+    assertTrue(m_scheduler.isRunning(command), "Command should be running when signal is true");
+
+    signal.set(false);
+    m_scheduler.run();
+    assertFalse(
+        m_scheduler.isRunning(command), "Command should be canceled when signal goes false");
+  }
+
+  @Test
+  void retryWhileTrueLongRunningRestarted() {
+    var signal = new AtomicBoolean(true);
+    var trigger = new Trigger(m_scheduler, signal::get);
+    var command = Command.noRequirements(Coroutine::park).named("Command");
+    trigger.retryWhileTrue(command);
+
+    m_scheduler.run();
+    assertTrue(m_scheduler.isRunning(command), "Command should be running initially");
+
+    m_scheduler.cancel(command);
+    m_scheduler.run();
+    assertTrue(m_scheduler.isRunning(command), "Command should be restarted in next cycle");
+  }
+
+  @Test
+  void retryWhileTrueOneShotRestarted() {
+    var signal = new AtomicBoolean(true);
+    var trigger = new Trigger(m_scheduler, signal::get);
+    var counter = new AtomicLong(0);
+    var oneshot = Command.noRequirements(_ -> counter.incrementAndGet()).named("One Shot");
+    trigger.retryWhileTrue(oneshot);
+
+    m_scheduler.run();
+    assertEquals(1, counter.get(), "Command should have run once");
+
+    m_scheduler.run();
+    assertEquals(2, counter.get(), "Command should have run twice");
+  }
+
+  @Test
+  void retryWhileFalseLongRunningCanceled() {
+    var signal = new AtomicBoolean(true);
+    var trigger = new Trigger(m_scheduler, signal::get);
+    var command = Command.noRequirements(Coroutine::park).named("Command");
+    trigger.retryWhileFalse(command);
+
+    signal.set(false);
+    m_scheduler.run();
+    assertTrue(m_scheduler.isRunning(command), "Command should be running when signal is false");
+
+    signal.set(true);
+    m_scheduler.run();
+    assertFalse(m_scheduler.isRunning(command), "Command should be canceled when signal goes true");
+  }
+
+  @Test
+  void retryWhileFalseLongRunningRestarted() {
+    var signal = new AtomicBoolean(false);
+    var trigger = new Trigger(m_scheduler, signal::get);
+    var command = Command.noRequirements(Coroutine::park).named("Command");
+    trigger.retryWhileFalse(command);
+
+    m_scheduler.run();
+    assertTrue(m_scheduler.isRunning(command), "Command should be running initially");
+
+    m_scheduler.cancel(command);
+    m_scheduler.run();
+    assertTrue(m_scheduler.isRunning(command), "Command should be restarted in next cycle");
+  }
+
+  @Test
+  void retryWhileFalseOneShotRestarted() {
+    var signal = new AtomicBoolean(false);
+    var trigger = new Trigger(m_scheduler, signal::get);
+    var counter = new AtomicLong(0);
+    var oneshot = Command.noRequirements(_ -> counter.incrementAndGet()).named("One Shot");
+    trigger.retryWhileFalse(oneshot);
+
+    m_scheduler.run();
+    assertEquals(1, counter.get(), "Command should have run once");
+
+    m_scheduler.run();
+    assertEquals(2, counter.get(), "Command should have run twice");
+  }
+
  private BooleanSupplier flickering(AtomicBoolean signal) {
    return () -> {
      boolean val = signal.get();