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..4298d57e8d1 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 @@ -159,6 +159,55 @@ public void run() { return this; } + /** + * 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; + } + + @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; + } + /** * Starts the given command when the condition changes to `false` and cancels it when the * condition changes to `true`.