diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java index d7b1dd08937..5cacc80d24d 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java @@ -10,6 +10,8 @@ import edu.wpi.first.wpilibj.event.EventLoop; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.WrapperCommand; + import java.util.function.BooleanSupplier; /** @@ -139,23 +141,60 @@ public void run() { */ public Trigger whileTrue(Command command) { requireNonNullParam(command, "command", "whileTrue"); - m_loop.bind( - new Runnable() { - private boolean m_pressedLast = m_condition.getAsBoolean(); - - @Override - public void run() { - boolean pressed = m_condition.getAsBoolean(); + return onTrue(new WrapperCommand(command) { + @Override + public boolean isFinished() { + return !m_condition.getAsBoolean() || super.isFinished(); + } + }); + } - if (!m_pressedLast && pressed) { - command.schedule(); - } else if (m_pressedLast && !pressed) { - command.cancel(); - } + /** + * Sets up a {@link Command} to mimic a default command while a condition is true. + * + *

The command will not interrupt any command other than the original default command of the + * subsystems the command requires. + * + * @param cmd the command to schedule + * @return this trigger, so calls can be chained + */ + public Trigger whileTrueLowPriority(Command cmd) { + // you could implement this by overiding the subsystems default command + // but that has alot of foot guns and likely would leak into causing issues + m_loop.bind( + new Runnable() { + // caching the sceduler to save slightly on fetching instance + private final CommandScheduler scheduler = CommandScheduler.getInstance(); + + public boolean freeToScehdule(Command cmd) { + var requirements = cmd.getRequirements(); + for (var requirement : requirements) { + // scheduler.requiring(requirement) returns a command or null + // requirement.getDefaultCommand() returns the default command or null + // + // logic assertions: + // - if both are null the command is free to schedule + // - if the default command is null and required command is not null the command + // is blocked from scheduling, so return false + // - required command should never be null if default command is not null + if (scheduler.requiring(requirement) != requirement.getDefaultCommand()) { + return false; + } + } + return true; + } - m_pressedLast = pressed; - } - }); + @Override + public void run() { + if (m_condition.getAsBoolean()) { + if (!scheduler.isScheduled(cmd) && freeToScehdule(cmd)) { + cmd.schedule(); + } + } else if (scheduler.isScheduled(cmd)) { + cmd.cancel(); + } + } + }); return this; } @@ -171,24 +210,12 @@ public void run() { */ public Trigger whileFalse(Command command) { requireNonNullParam(command, "command", "whileFalse"); - m_loop.bind( - new Runnable() { - private boolean m_pressedLast = m_condition.getAsBoolean(); - - @Override - public void run() { - boolean pressed = m_condition.getAsBoolean(); - - if (m_pressedLast && !pressed) { - command.schedule(); - } else if (!m_pressedLast && pressed) { - command.cancel(); - } - - m_pressedLast = pressed; - } - }); - return this; + return onFalse(new WrapperCommand(command) { + @Override + public boolean isFinished() { + return m_condition.getAsBoolean() || super.isFinished(); + } + }); } /**