diff --git a/commandsv3/src/main/java/org/wpilib/command3/Trigger.java b/commandsv3/src/main/java/org/wpilib/command3/Trigger.java index 2a3a99db1d..d78b92769a 100644 --- 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`. * - *

Doesn't re-start the command if it ends while the condition is still `true`. + *

Unlike {@link #retryWhileTrue(Command)}, this does not 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`. * - *

Doesn't re-start the command if it ends while the condition is still `false`. + *

Unlike {@link #retryWhileFalse(Command)}, this does not 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`. + * + *

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

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 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); } } diff --git a/commandsv3/src/test/java/org/wpilib/command3/TriggerTest.java b/commandsv3/src/test/java/org/wpilib/command3/TriggerTest.java index d5ed5329cd..b6cbdd75b9 100644 --- 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();