Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-22 01:11:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[commands] Revamp Interruptible (#4192)

Browse Source
This commit is contained in:
Starlight220
2022-08-30 07:53:47 +03:00
committed by GitHub
parent f2a8d38d2a
commit c3a93fb995
26 changed files with 369 additions and 592 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -47,14 +47,15 @@ public interface Command {
/**
* Specifies the set of subsystems used by this command. Two commands cannot use the same
* subsystem at the same time. If the command is scheduled as interruptible and another command is
* scheduled that shares a requirement, the command will be interrupted. Else, the command will
* not be scheduled. If no subsystems are required, return an empty set.
* subsystem at the same time. If another command is scheduled that shares a requirement, {@link
* #getInterruptionBehavior()} will be checked and followed. If no subsystems are required, return
* an empty set.
*
* <p>Note: it is recommended that user implementations contain the requirements as a field, and
* return that field here, rather than allocating a new set every time this is called.
*
* @return the set of subsystems that are required
* @see InterruptionBehavior
*/
Set<Subsystem> getRequirements();
@@ -331,23 +332,30 @@ public interface Command {
}
/**
* Schedules this command.
* Decorates this command to have a different {@link InterruptionBehavior interruption behavior}.
*
* @param interruptible whether this command can be interrupted by another command that shares one
* of its requirements
* @param interruptBehavior the desired interrupt behavior
* @return the decorated command
*/
default void schedule(boolean interruptible) {
CommandScheduler.getInstance().schedule(interruptible, this);
default WrapperCommand withInterruptBehavior(InterruptionBehavior interruptBehavior) {
return new WrapperCommand(this) {
@Override
public InterruptionBehavior getInterruptionBehavior() {
return interruptBehavior;
}
};
}
/** Schedules this command, defaulting to interruptible. */
/** Schedules this command. */
default void schedule() {
schedule(true);
CommandScheduler.getInstance().schedule(this);
}
/**
* Cancels this command. Will call the command's end() method with interrupted=true. Commands will
* be canceled even if they are not marked as interruptible.
* Cancels this command. Will call {@link #end(boolean) end(true)}. Commands will be canceled
* regardless of {@link InterruptionBehavior interruption behavior}.
*
* @see CommandScheduler#cancel(Command...)
*/
default void cancel() {
CommandScheduler.getInstance().cancel(this);
@@ -373,6 +381,16 @@ public interface Command {
return getRequirements().contains(requirement);
}
/**
* How the command behaves when another command with a shared requirement is scheduled.
*
* @return a variant of {@link InterruptionBehavior}, defaulting to {@link
* InterruptionBehavior#kCancelSelf kCancelSelf}.
*/
default InterruptionBehavior getInterruptionBehavior() {
return InterruptionBehavior.kCancelSelf;
}
/**
* Whether the given command should run when the robot is disabled. Override to return true if the
* command should run when disabled.
@@ -391,4 +409,20 @@ public interface Command {
default String getName() {
return this.getClass().getSimpleName();
}
/**
* An enum describing the command's behavior when another command with a shared requirement is
* scheduled.
*/
enum InterruptionBehavior {
/**
* This command ends, {@link #end(boolean) end(true)} is called, and the incoming command is
* scheduled normally.
*
* <p>This is the default behavior.
*/
kCancelSelf,
/** This command continues, and the incoming command is not scheduled. */
kCancelIncoming
}
}

102
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java
View File

@@ -20,14 +20,14 @@ import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.Watchdog;
import edu.wpi.first.wpilibj.event.EventLoop;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior;
import java.util.ArrayList;
import java.util.Collections;
import java.util.Iterator;
import java.util.LinkedHashMap;
import java.util.LinkedHashSet;
import java.util.List;
import java.util.Map;
import java.util.Optional;
import java.util.Set;
import java.util.function.Consumer;
@@ -56,11 +56,10 @@ public final class CommandScheduler implements NTSendable, AutoCloseable {
return instance;
}
// A map from commands to their scheduling state. Also used as a set of the currently-running
// commands.
private final Map<Command, CommandState> m_scheduledCommands = new LinkedHashMap<>();
// A set of the currently-running commands.
private final Set<Command> m_scheduledCommands = new LinkedHashSet<>();
// A map from required subsystems to their requiring commands. Also used as a set of the
// A map from required subsystems to their requiring commands. Also used as a set of the
// currently-required subsystems.
private final Map<Subsystem, Command> m_requirements = new LinkedHashMap<>();
@@ -83,7 +82,7 @@ public final class CommandScheduler implements NTSendable, AutoCloseable {
// Flag and queues for avoiding ConcurrentModificationException if commands are
// scheduled/canceled during run
private boolean m_inRunLoop;
private final Map<Command, Boolean> m_toSchedule = new LinkedHashMap<>();
private final Set<Command> m_toSchedule = new LinkedHashSet<>();
private final List<Command> m_toCancel = new ArrayList<>();
private final Watchdog m_watchdog = new Watchdog(TimedRobot.kDefaultPeriod, () -> {});
@@ -103,7 +102,7 @@ public final class CommandScheduler implements NTSendable, AutoCloseable {
}
/**
* Changes the period of the loop overrun watchdog. This should be be kept in sync with the
* Changes the period of the loop overrun watchdog. This should be kept in sync with the
* TimedRobot period.
*
* @param period Period in seconds.
@@ -151,7 +150,7 @@ public final class CommandScheduler implements NTSendable, AutoCloseable {
* Adds a button binding to the scheduler, which will be polled to schedule commands.
*
* @param button The button to add
* @deprecated Use {@link Trigger}
* @deprecated Use {@link edu.wpi.first.wpilibj2.command.button.Trigger}
*/
@Deprecated(since = "2023")
public void addButton(Runnable button) {
@@ -172,12 +171,10 @@ public final class CommandScheduler implements NTSendable, AutoCloseable {
* Initializes a given command, adds its requirements to the list, and performs the init actions.
*
* @param command The command to initialize
* @param interruptible Whether the command is interruptible
* @param requirements The command requirements
*/
private void initCommand(Command command, boolean interruptible, Set<Subsystem> requirements) {
CommandState scheduledCommand = new CommandState(interruptible);
m_scheduledCommands.put(command, scheduledCommand);
private void initCommand(Command command, Set<Subsystem> requirements) {
m_scheduledCommands.add(command);
for (Subsystem requirement : requirements) {
m_requirements.put(requirement, command);
}
@@ -195,17 +192,15 @@ public final class CommandScheduler implements NTSendable, AutoCloseable {
* using those requirements have been scheduled as interruptible. If this is the case, they will
* be interrupted and the command will be scheduled.
*
* @param interruptible whether this command can be interrupted.
* @param command the command to schedule. If null, no-op.
*/
private void schedule(boolean interruptible, Command command) {
private void schedule(Command command) {
if (command == null) {
DriverStation.reportWarning("Tried to schedule a null command", true);
return;
}
if (m_inRunLoop) {
m_toSchedule.put(command, interruptible);
m_toSchedule.add(command);
return;
}
@@ -226,16 +221,14 @@ public final class CommandScheduler implements NTSendable, AutoCloseable {
// Schedule the command if the requirements are not currently in-use.
if (Collections.disjoint(m_requirements.keySet(), requirements)) {
initCommand(command, interruptible, requirements);
initCommand(command, requirements);
} else {
// Else check if the requirements that are in use have all have interruptible commands,
// and if so, interrupt those commands and schedule the new command.
for (Subsystem requirement : requirements) {
Command requiring = requiring(requirement);
if (requiring != null
&& !Optional.ofNullable(m_scheduledCommands.get(requiring))
.map(CommandState::isInterruptible)
.orElse(true)) {
&& requiring.getInterruptionBehavior() == InterruptionBehavior.kCancelIncoming) {
return;
}
}
@@ -245,33 +238,19 @@ public final class CommandScheduler implements NTSendable, AutoCloseable {
cancel(requiring);
}
}
initCommand(command, interruptible, requirements);
initCommand(command, requirements);
}
}
/**
* Schedules multiple commands for execution. Does nothing if the command is already scheduled. If
* a command's requirements are not available, it will only be started if all the commands
* currently using those requirements have been scheduled as interruptible. If this is the case,
* they will be interrupted and the command will be scheduled.
*
* @param interruptible whether the commands should be interruptible
* @param commands the commands to schedule. No-op if null.
*/
public void schedule(boolean interruptible, Command... commands) {
for (Command command : commands) {
schedule(interruptible, command);
}
}
/**
* Schedules multiple commands for execution, with interruptible defaulted to true. Does nothing
* if the command is already scheduled.
* Schedules multiple commands for execution. Does nothing for commands already scheduled.
*
* @param commands the commands to schedule. No-op on null.
*/
public void schedule(Command... commands) {
schedule(true, commands);
for (Command command : commands) {
schedule(command);
}
}
/**
@@ -312,8 +291,7 @@ public final class CommandScheduler implements NTSendable, AutoCloseable {
m_inRunLoop = true;
// Run scheduled commands, remove finished commands.
for (Iterator<Command> iterator = m_scheduledCommands.keySet().iterator();
iterator.hasNext(); ) {
for (Iterator<Command> iterator = m_scheduledCommands.iterator(); iterator.hasNext(); ) {
Command command = iterator.next();
if (!command.runsWhenDisabled() && RobotState.isDisabled()) {
@@ -346,8 +324,8 @@ public final class CommandScheduler implements NTSendable, AutoCloseable {
m_inRunLoop = false;
// Schedule/cancel commands from queues populated during loop
for (Map.Entry<Command, Boolean> commandInterruptible : m_toSchedule.entrySet()) {
schedule(commandInterruptible.getValue(), commandInterruptible.getKey());
for (Command command : m_toSchedule) {
schedule(command);
}
for (Command command : m_toCancel) {
@@ -427,6 +405,14 @@ public final class CommandScheduler implements NTSendable, AutoCloseable {
throw new IllegalArgumentException("Default commands should not end!");
}
if (defaultCommand.getInterruptionBehavior() == InterruptionBehavior.kCancelIncoming) {
DriverStation.reportWarning(
"Registering a non-interruptible default command!\n"
+ "This will likely prevent any other commands from requiring this subsystem.",
true);
// Warn, but allow -- there might be a use case for this.
}
m_subsystems.put(subsystem, defaultCommand);
}
@@ -446,7 +432,7 @@ public final class CommandScheduler implements NTSendable, AutoCloseable {
* canceled command with {@code true}, indicating they were canceled (as opposed to finishing
* normally).
*
* <p>Commands will be canceled even if they are not scheduled as interruptible.
* <p>Commands will be canceled regardless of {@link InterruptionBehavior interruption behavior}.
*
* @param commands the commands to cancel
*/
@@ -477,26 +463,8 @@ public final class CommandScheduler implements NTSendable, AutoCloseable {
/** Cancels all commands that are currently scheduled. */
public void cancelAll() {
for (Command command : m_scheduledCommands.keySet().toArray(new Command[0])) {
cancel(command);
}
}
/**
* Returns the time since a given command was scheduled. Note that this only works on commands
* that are directly scheduled by the scheduler; it will not work on commands inside of
* commandgroups, as the scheduler does not see them.
*
* @param command the command to query
* @return the time since the command was scheduled, in seconds
*/
public double timeSinceScheduled(Command command) {
CommandState commandState = m_scheduledCommands.get(command);
if (commandState != null) {
return commandState.timeSinceInitialized();
} else {
return -1;
}
// Copy to array to avoid concurrent modification.
cancel(m_scheduledCommands.toArray(new Command[0]));
}
/**
@@ -508,7 +476,7 @@ public final class CommandScheduler implements NTSendable, AutoCloseable {
* @return whether the command is currently scheduled
*/
public boolean isScheduled(Command... commands) {
return m_scheduledCommands.keySet().containsAll(Set.of(commands));
return m_scheduledCommands.containsAll(Set.of(commands));
}
/**
@@ -583,7 +551,7 @@ public final class CommandScheduler implements NTSendable, AutoCloseable {
Map<Double, Command> ids = new LinkedHashMap<>();
for (Command command : m_scheduledCommands.keySet()) {
for (Command command : m_scheduledCommands) {
ids.put((double) command.hashCode(), command);
}

42
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandState.java
View File

@@ -1,42 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj2.command;
import edu.wpi.first.wpilibj.Timer;
/**
* Class that holds scheduling state for a command. Used internally by the {@link CommandScheduler}.
*
* <p>This class is provided by the NewCommands VendorDep
*/
class CommandState {
// The time since this command was initialized.
private double m_startTime = -1;
// Whether or not it is interruptible.
private final boolean m_interruptible;
CommandState(boolean interruptible) {
m_interruptible = interruptible;
startTiming();
startRunning();
}
private void startTiming() {
m_startTime = Timer.getFPGATimestamp();
}
synchronized void startRunning() {
m_startTime = -1;
}
boolean isInterruptible() {
return m_interruptible;
}
double timeSinceInitialized() {
return m_startTime != -1 ? Timer.getFPGATimestamp() - m_startTime : -1;
}
}

5
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WrapperCommand.java
View File

@@ -95,6 +95,11 @@ public abstract class WrapperCommand implements Command {
return m_command.runsWhenDisabled();
}
@Override
public InterruptionBehavior getInterruptionBehavior() {
return m_command.getInterruptionBehavior();
}
/**
* Gets the name of this Command.
*

72
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Button.java
View File

@@ -40,19 +40,6 @@ public class Button extends Trigger {
* Starts the given command whenever the button is newly pressed.
*
* @param command the command to start
* @param interruptible whether the command is interruptible
* @return this button, so calls can be chained
*/
public Button whenPressed(final Command command, boolean interruptible) {
whenActive(command, interruptible);
return this;
}
/**
* Starts the given command whenever the button is newly pressed. The command is set to be
* interruptible.
*
* @param command the command to start
* @return this button, so calls can be chained
*/
public Button whenPressed(final Command command) {
@@ -75,23 +62,8 @@ public class Button extends Trigger {
/**
* Constantly starts the given command while the button is held.
*
* <p>{@link Command#schedule(boolean)} will be called repeatedly while the button is held, and
* will be canceled when the button is released.
*
* @param command the command to start
* @param interruptible whether the command is interruptible
* @return this button, so calls can be chained
*/
public Button whileHeld(final Command command, boolean interruptible) {
whileActiveContinuous(command, interruptible);
return this;
}
/**
* Constantly starts the given command while the button is held.
*
* <p>{@link Command#schedule(boolean)} will be called repeatedly while the button is held, and
* will be canceled when the button is released. The command is set to be interruptible.
* <p>{@link Command#schedule()} will be called repeatedly while the button is held, and will be
* canceled when the button is released.
*
* @param command the command to start
* @return this button, so calls can be chained
@@ -118,36 +90,10 @@ public class Button extends Trigger {
* but does not start it again if it ends or is otherwise interrupted.
*
* @param command the command to start
* @param interruptible whether the command is interruptible
* @return this button, so calls can be chained
*/
public Button whenHeld(final Command command, boolean interruptible) {
whileActiveOnce(command, interruptible);
return this;
}
/**
* Starts the given command when the button is first pressed, and cancels it when it is released,
* but does not start it again if it ends or is otherwise interrupted. The command is set to be
* interruptible.
*
* @param command the command to start
* @return this button, so calls can be chained
*/
public Button whenHeld(final Command command) {
whileActiveOnce(command, true);
return this;
}
/**
* Starts the command when the button is released.
*
* @param command the command to start
* @param interruptible whether the command is interruptible
* @return this button, so calls can be chained
*/
public Button whenReleased(final Command command, boolean interruptible) {
whenInactive(command, interruptible);
whileActiveOnce(command);
return this;
}
@@ -174,18 +120,6 @@ public class Button extends Trigger {
return this;
}
/**
* Toggles the command whenever the button is pressed (on then off then on).
*
* @param command the command to start
* @param interruptible whether the command is interruptible
* @return this button, so calls can be chained
*/
public Button toggleWhenPressed(final Command command, boolean interruptible) {
toggleWhenActive(command, interruptible);
return this;
}
/**
* Toggles the command whenever the button is pressed (on then off then on). The command is set to
* be interruptible.

102
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java
View File

@@ -80,25 +80,13 @@ public class Trigger extends BooleanEvent {
* Starts the given command whenever the trigger just becomes active.
*
* @param command the command to start
* @param interruptible whether the command is interruptible
* @return this trigger, so calls can be chained
*/
public Trigger whenActive(final Command command, boolean interruptible) {
requireNonNullParam(command, "command", "whenActive");
this.rising().ifHigh(() -> command.schedule(interruptible));
return this;
}
/**
* Starts the given command whenever the trigger just becomes active. The command is set to be
* interruptible.
*
* @param command the command to start
* @return this trigger, so calls can be chained
*/
public Trigger whenActive(final Command command) {
return whenActive(command, true);
requireNonNullParam(command, "command", "whenActive");
this.rising().ifHigh(command::schedule);
return this;
}
/**
@@ -115,33 +103,19 @@ public class Trigger extends BooleanEvent {
/**
* Constantly starts the given command while the button is held.
*
* <p>{@link Command#schedule(boolean)} will be called repeatedly while the trigger is active, and
* will be canceled when the trigger becomes inactive.
*
* @param command the command to start
* @param interruptible whether the command is interruptible
* @return this trigger, so calls can be chained
*/
public Trigger whileActiveContinuous(final Command command, boolean interruptible) {
requireNonNullParam(command, "command", "whileActiveContinuous");
this.ifHigh(() -> command.schedule(interruptible));
this.falling().ifHigh(command::cancel);
return this;
}
/**
* Constantly starts the given command while the button is held.
*
* <p>{@link Command#schedule(boolean)} will be called repeatedly while the trigger is active, and
* will be canceled when the trigger becomes inactive.
* <p>{@link Command#schedule()} will be called repeatedly while the trigger is active, and will
* be canceled when the trigger becomes inactive.
*
* @param command the command to start
* @return this trigger, so calls can be chained
*/
public Trigger whileActiveContinuous(final Command command) {
return whileActiveContinuous(command, true);
requireNonNullParam(command, "command", "whileActiveContinuous");
this.ifHigh(command::schedule);
this.falling().ifHigh(command::cancel);
return this;
}
/**
@@ -160,52 +134,29 @@ public class Trigger extends BooleanEvent {
* inactive, but does not re-start it in-between.
*
* @param command the command to start
* @param interruptible whether the command is interruptible
* @return this trigger, so calls can be chained
*/
public Trigger whileActiveOnce(final Command command, boolean interruptible) {
public Trigger whileActiveOnce(final Command command) {
requireNonNullParam(command, "command", "whileActiveOnce");
this.rising().ifHigh(() -> command.schedule(interruptible));
this.rising().ifHigh(command::schedule);
this.falling().ifHigh(command::cancel);
return this;
}
/**
* Starts the given command when the trigger initially becomes active, and ends it when it becomes
* inactive, but does not re-start it in-between. The command is set to be interruptible.
*
* @param command the command to start
* @return this trigger, so calls can be chained
*/
public Trigger whileActiveOnce(final Command command) {
return whileActiveOnce(command, true);
}
/**
* Starts the command when the trigger becomes inactive.
*
* @param command the command to start
* @param interruptible whether the command is interruptible
* @return this trigger, so calls can be chained
*/
public Trigger whenInactive(final Command command, boolean interruptible) {
requireNonNullParam(command, "command", "whenInactive");
this.falling().ifHigh(() -> command.schedule(interruptible));
return this;
}
/**
* Starts the command when the trigger becomes inactive. The command is set to be interruptible.
*
* @param command the command to start
* @return this trigger, so calls can be chained
*/
public Trigger whenInactive(final Command command) {
return whenInactive(command, true);
requireNonNullParam(command, "command", "whenInactive");
this.falling().ifHigh(command::schedule);
return this;
}
/**
@@ -223,10 +174,9 @@ public class Trigger extends BooleanEvent {
* Toggles a command when the trigger becomes active.
*
* @param command the command to toggle
* @param interruptible whether the command is interruptible
* @return this trigger, so calls can be chained
*/
public Trigger toggleWhenActive(final Command command, boolean interruptible) {
public Trigger toggleWhenActive(final Command command) {
requireNonNullParam(command, "command", "toggleWhenActive");
this.rising()
@@ -235,23 +185,13 @@ public class Trigger extends BooleanEvent {
if (command.isScheduled()) {
command.cancel();
} else {
command.schedule(interruptible);
command.schedule();
}
});
return this;
}
/**
* Toggles a command when the trigger becomes active. The command is set to be interruptible.
*
* @param command the command to toggle
* @return this trigger, so calls can be chained
*/
public Trigger toggleWhenActive(final Command command) {
return toggleWhenActive(command, true);
}
/**
* Cancels a command when the trigger becomes active.
*