Move ending command removal to after callbacks

wpilibsuite · Aug 4, 2023 · aee5266 · aee5266
1 parent 0ab2c4a
commit aee5266
Showing 1 changed file with 4 additions and 4 deletions.
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java
@@ -86,7 +86,7 @@ public static synchronized CommandScheduler getInstance() {
   // scheduled/canceled during run
   private boolean m_inRunLoop;
   private final Set<Command> m_toSchedule = new LinkedHashSet<>();
-  private final Set<Command> m_toCancel = new LinkedHashSet<>();
+  private final List<Command> m_toCancel = new ArrayList<>();
   private final Set<Command> m_ending = new LinkedHashSet<>();
 
   private final Watchdog m_watchdog = new Watchdog(TimedRobot.kDefaultPeriod, () -> {});
@@ -275,10 +275,10 @@ public void run() {
       if (!command.runsWhenDisabled() && isDisabled) {
         m_ending.add(command);
         command.end(true);
-        m_ending.remove(command);
         for (Consumer<Command> action : m_interruptActions) {
           action.accept(command);
         }
+        m_ending.remove(command);
         iterator.remove();
         m_requirements.keySet().removeAll(command.getRequirements());
         m_watchdog.addEpoch(command.getName() + ".end(true)");
@@ -293,10 +293,10 @@ public void run() {
       if (command.isFinished()) {
         m_ending.add(command);
         command.end(false);
-        m_ending.remove(command);
         for (Consumer<Command> action : m_finishActions) {
           action.accept(command);
         }
+        m_ending.remove(command);
         iterator.remove();
 
         m_requirements.keySet().removeAll(command.getRequirements());
@@ -465,10 +465,10 @@ public void cancel(Command... commands) {
 
       m_ending.add(command);
       command.end(true);
-      m_ending.remove(command);
       for (Consumer<Command> action : m_interruptActions) {
         action.accept(command);
       }
+      m_ending.remove(command);
       m_scheduledCommands.remove(command);
       m_requirements.keySet().removeAll(command.getRequirements());
       m_watchdog.addEpoch(command.getName() + ".end(true)");