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

[commands] Make command scheduling order consistent (#5470)

Browse Source
This commit is contained in:
Gold856
2023-10-10 02:52:26 -04:00
committed by GitHub
parent 58e8474368
commit ff1849052e
4 changed files with 569 additions and 67 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -92,6 +92,7 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
private final Set<Command> m_toSchedule = new LinkedHashSet<>();
private final List<Command> m_toCancelCommands = new ArrayList<>();
private final List<Optional<Command>> m_toCancelInterruptors = new ArrayList<>();
private final Set<Command> m_endingCommands = new LinkedHashSet<>();
private final Watchdog m_watchdog = new Watchdog(TimedRobot.kDefaultPeriod, () -> {});
@@ -271,18 +272,13 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
m_watchdog.addEpoch("buttons.run()");
m_inRunLoop = true;
boolean isDisabled = RobotState.isDisabled();
// Run scheduled commands, remove finished commands.
for (Iterator<Command> iterator = m_scheduledCommands.iterator(); iterator.hasNext(); ) {
Command command = iterator.next();
if (!command.runsWhenDisabled() && RobotState.isDisabled()) {
command.end(true);
for (BiConsumer<Command, Optional<Command>> action : m_interruptActions) {
action.accept(command, kNoInterruptor);
}
m_requirements.keySet().removeAll(command.getRequirements());
iterator.remove();
m_watchdog.addEpoch(command.getName() + ".end(true)");
if (isDisabled && !command.runsWhenDisabled()) {
cancel(command, kNoInterruptor);
continue;
}
@@ -292,10 +288,12 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
}
m_watchdog.addEpoch(command.getName() + ".execute()");
if (command.isFinished()) {
m_endingCommands.add(command);
command.end(false);
for (Consumer<Command> action : m_finishActions) {
action.accept(command);
}
m_endingCommands.remove(command);
iterator.remove();
m_requirements.keySet().removeAll(command.getRequirements());
@@ -466,6 +464,9 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
DriverStation.reportWarning("Tried to cancel a null command", true);
return;
}
if (m_endingCommands.contains(command)) {
return;
}
if (m_inRunLoop) {
m_toCancelCommands.add(command);
m_toCancelInterruptors.add(interruptor);
@@ -475,12 +476,14 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
return;
}
m_scheduledCommands.remove(command);
m_requirements.keySet().removeAll(command.getRequirements());
m_endingCommands.add(command);
command.end(true);
for (BiConsumer<Command, Optional<Command>> action : m_interruptActions) {
action.accept(command, interruptor);
}
m_endingCommands.remove(command);
m_scheduledCommands.remove(command);
m_requirements.keySet().removeAll(command.getRequirements());
m_watchdog.addEpoch(command.getName() + ".end(true)");
}