[cmd3] Enforce command lifetimes across all opmode and command scopes (#8705)

Commands are no longer able to outlive their schedule-site's scope,
regardless of how they were scheduled (set as a default command, bound
to a trigger, or manually scheduled)

As a consequence, default commands need better tracking so the default
command setting can be released when their scope exits and the next-most
appropriate default command can be rescheduled (eg, an opmode sets a
default command, then the globally-scoped default is restored when the
opmode exits). Some complexity is required here to make it work well for
edge cases.

Like `schedule()`, `setDefaultCommand()` will immediately start the new
default command if called inside of another command to avoid 1-loop
delays. However, this does not apply when called by the _current_
default command, as it would result in attempting to cancel the default
command while it's mounted (which is impossible and would throw an
exception)

```java
class Robot extends OpModeRobot {
  final Drive drive = new Drive();
  final CommandXboxController controller = new CommandXboxController(1);

  public Robot() {
    // global default command, active unless overridden in an opmode or command
    drive.setDefaultCommand(drive.stop());

    // global trigger binding, always active
    controller.rightBumper().onTrue(drive.setX());
  }
}

@Teleop
class ExampleOpMode extends PeriodicOpMode {
  public ExampleOpMode(Robot robot) {
    // opmode-specific default command
    robot.drive.setDefaultCommand(robot.drive.operatorControl(robot.controller));

    // opmode-specific binding
    robot.controller.leftBumper().whileTrue(robot.drive.stop());

    // opmode-specific binding that takes precedence over the global binding
    // because it happens last; it "wins out" over the `setX()` binding
    robot.controller.rightBumper().onTrue(robot.drive.selfTest());
  }

  @Override
  public void periodic() {
    Scheduler.getDefault().run();
  }
}
```

commandsv3/src/main/java/org/wpilib/command3/BindingScope.java

@@ -17,20 +17,24 @@ interface BindingScope {
    */
   boolean active();
 
-  static BindingScope global() {
-    return Global.INSTANCE;
-  }
+  /**
+   * Creates the narrowest binding scope available based on the current state of the scheduler and
+   * selected opmode.
+   *
+   * @param scheduler The scheduler to create the binding scope for.
+   * @return The narrowest binding scope available.
+   */
+  static BindingScope createNarrowestScope(Scheduler scheduler) {
+    Command currentCommand = scheduler.currentCommand();
+    long currentOpMode = OpModeFetcher.getFetcher().getOpModeId();
 
-  static BindingScope forCommand(Scheduler scheduler, Command command) {
-    return new ForCommand(scheduler, command);
-  }
-
-  static BindingScope forOpmode(long opmodeId) {
-    if (opmodeId == 0) {
-      throw new IllegalArgumentException("Invalid OpMode ID provided");
+    if (currentCommand != null) {
+      return new ForCommand(scheduler, currentCommand);
+    } else if (currentOpMode != 0) {
+      return new ForOpmode(currentOpMode);
+    } else {
+      return Global.INSTANCE;
     }
-
-    return new ForOpmode(opmodeId);
   }
 
   /** A global binding scope. Bindings in this scope are always active. */

commandsv3/src/main/java/org/wpilib/command3/BindingType.java

@@ -42,5 +42,9 @@ enum BindingType {
    * rising edge, it will be canceled then - unlike {@link #SCHEDULE_ON_FALLING_EDGE}, which would
    * allow it to continue to run.
    */
-  RUN_WHILE_LOW
+  RUN_WHILE_LOW,
+  /** Continuously attempts to schedule a command as long as the signal remains high. */
+  CONTINUOUSLY_SCHEDULE_WHILE_HIGH,
+  /** Continuously attempts to schedule a command as long as the signal remains low. */
+  CONTINUOUSLY_SCHEDULE_WHILE_LOW
 }

commandsv3/src/main/java/org/wpilib/command3/Scheduler.java

@@ -95,7 +95,18 @@ import org.wpilib.util.protobuf.ProtobufSerializable;
  * protobuf serializer. However, it is up to the user to log those events themselves.
  */
 public final class Scheduler implements ProtobufSerializable {
-  private final Map<Mechanism, Command> m_defaultCommands = new LinkedHashMap<>();
+  /**
+   * The default command bindings for each mechanism. Binding lists are ordered by priority; the
+   * last element in the list is the highest priority default command to be used. Bindings need to
+   * be periodically checked and removed when they're inactive.
+   */
+  private final Map<Mechanism, List<Binding>> m_defaultCommandBindings = new LinkedHashMap<>();
+
+  /**
+   * All bindings attached to this scheduler. This lets us cancel commands tied to scopes that go
+   * inactive. Bindings need to be periodically checked and removed when they're inactive.
+   */
+  private final Collection<Binding> m_activeBindings = new ArrayList<>();
 
   /** The set of commands scheduled since the start of the previous run. */
   private final SequencedSet<CommandState> m_queuedToRun = new LinkedHashSet<>();
@@ -164,14 +175,24 @@ public final class Scheduler implements ProtobufSerializable {
   private Scheduler() {}
 
   /**
-   * Sets the default command for a mechanism. The command must require that mechanism, and cannot
-   * require any other mechanisms.
+   * Sets the default command for a mechanism. The command must require that mechanism and cannot
+   * require any other mechanisms. If another default command has already been set for this
+   * mechanism, the one provided will supersede it.
+   *
+   * <p>If this is called inside a running opmode or in a running command, the default command
+   * setting will only apply while that opmode or command is active. When the opmode or command
+   * exits, the previous default command setting will be restored.
+   *
+   * <p>Commands running as default commands may call this method to change their mechanism's
+   * default command on the fly. The new default command will take effect at the end of the
+   * scheduler's loop cycle.
    *
    * @param mechanism the mechanism for which to set the default command
    * @param defaultCommand the default command to execute on the mechanism
    * @throws IllegalArgumentException if the command does not meet the requirements for being a
    *     default command
    */
+  @SuppressWarnings("PMD.CompareObjectsWithEquals")
   public void setDefaultCommand(Mechanism mechanism, Command defaultCommand) {
     if (!defaultCommand.requires(mechanism)) {
       throw new IllegalArgumentException(
@@ -183,17 +204,51 @@ public final class Scheduler implements ProtobufSerializable {
           "A mechanism's default command cannot require other mechanisms");
     }
 
-    m_defaultCommands.put(mechanism, defaultCommand);
+    var currentCommand = currentCommand();
+    BindingScope scope = BindingScope.createNarrowestScope(this);
+
+    var binding =
+        new Binding(
+            scope,
+            BindingType.CONTINUOUSLY_SCHEDULE_WHILE_HIGH,
+            defaultCommand,
+            new Throwable().getStackTrace());
+
+    var currentDefaultCommand = getDefaultCommandFor(mechanism);
+    m_defaultCommandBindings.computeIfAbsent(mechanism, k -> new ArrayList<>()).add(binding);
+
+    if (currentCommand != null && currentCommand != currentDefaultCommand) {
+      // User called `setDefaultCommand` inside another command.
+      // Immediately reprocess the default commands for this mechanism to ensure it's in sync with
+      // the rest of the commands in the scheduler. This is required because we normally schedule
+      // the default commands at the start of the scheduler `run()` method, so the new default
+      // command wouldn't be handled until the next run (ie, the previous default command would
+      // still be active for the current iteration)
+      //
+      // Note that we cannot do this if the current default command is the caller because commands
+      // cannot be canceled while mounted.
+      processDefaultCommands(mechanism);
+    }
   }
 
   /**
-   * Gets the default command set for a mechanism.
+   * Gets the default command currently used for a mechanism.
    *
    * @param mechanism The mechanism
    * @return The default command, or null if no default command was ever set
    */
   public Command getDefaultCommandFor(Mechanism mechanism) {
-    return m_defaultCommands.get(mechanism);
+    var bindings = m_defaultCommandBindings.getOrDefault(mechanism, Collections.emptyList());
+    if (bindings.isEmpty()) {
+      return null;
+    }
+
+    return bindings.getLast().command();
+  }
+
+  // package-private helper for unit test access
+  List<Binding> getDefaultCommandBindingsFor(Mechanism mechanism) {
+    return m_defaultCommandBindings.getOrDefault(mechanism, Collections.emptyList());
   }
 
   /**
@@ -275,17 +330,7 @@ public final class Scheduler implements ProtobufSerializable {
     // This prevents commands from outliving the opmodes that scheduled them, or from outliving
     // their parents (eg if someone writes a command that manually calls schedule(Command) instead
     // of using triggers to do so).
-    Command currentCommand = currentCommand();
-    long currentOpmode = OpModeFetcher.getFetcher().getOpModeId();
-
-    BindingScope scope;
-    if (currentCommand != null) {
-      scope = BindingScope.forCommand(this, currentCommand);
-    } else if (currentOpmode != 0) {
-      scope = BindingScope.forOpmode(currentOpmode);
-    } else {
-      scope = BindingScope.global();
-    }
+    BindingScope scope = BindingScope.createNarrowestScope(this);
 
     // Note: we use a throwable here instead of Thread.currentThread().getStackTrace() for easier
     //       stack frame filtering and modification.
@@ -319,6 +364,11 @@ public final class Scheduler implements ProtobufSerializable {
       }
     }
 
+    // Track this binding so we can disable it when it's out of scope.
+    // Note that, even though triggers can clean themselves up, commands that are manually scheduled
+    // cannot do the same, so we have to track them in the scheduler.
+    m_activeBindings.add(binding);
+
     // Evict conflicting on-deck commands
     // We check above if the input command is lower priority than any of these,
     // so at this point we're guaranteed to be >= priority than anything already on deck
@@ -491,6 +541,8 @@ public final class Scheduler implements ProtobufSerializable {
    * Updates the command scheduler. This will run operations in the following order:
    *
    * <ol>
+   *   <li>Cancel any commands bound to scopes that have gone inactive, such as having been
+   *       scheduled in an opmode that's no longer selected on the driverstation
    *   <li>Run sideloaded functions from {@link #sideload(Consumer)} and {@link
    *       #addPeriodic(Runnable)}
    *   <li>Update trigger bindings to queue and cancel bound commands
@@ -506,6 +558,9 @@ public final class Scheduler implements ProtobufSerializable {
   public void run() {
     final long startMicros = RobotController.getTime();
 
+    // Cancel any commands with stale binding scopes
+    cancelStaleBindings();
+
     // Sideloads may change some state that affects triggers. Run them first.
     runPeriodicSideloads();
 
@@ -526,6 +581,17 @@ public final class Scheduler implements ProtobufSerializable {
     m_lastRunTimeMs = Milliseconds.convertFrom(endMicros - startMicros, Microseconds);
   }
 
+  private void cancelStaleBindings() {
+    for (var iterator = m_activeBindings.iterator(); iterator.hasNext(); ) {
+      var binding = iterator.next();
+      if (binding.scope().active()) {
+        continue;
+      }
+      cancel(binding.command());
+      iterator.remove();
+    }
+  }
+
   private void promoteScheduledCommands() {
     // Clear any commands that conflict with the scheduled set
     for (var queuedState : m_queuedToRun) {
@@ -696,18 +762,49 @@ public final class Scheduler implements ProtobufSerializable {
   }
 
   private void scheduleDefaultCommands() {
-    // Schedule the default commands for every mechanism that doesn't currently have a running or
-    // scheduled command.
-    m_defaultCommands.forEach(
-        (mechanism, defaultCommand) -> {
-          if (m_runningCommands.keySet().stream().noneMatch(c -> c.requires(mechanism))
-              && m_queuedToRun.stream().noneMatch(c -> c.command().requires(mechanism))
-              && defaultCommand != null) {
-            // Nothing currently running or scheduled
-            // Schedule the mechanism's default command, if it has one
-            schedule(defaultCommand);
+    m_defaultCommandBindings.keySet().forEach(this::processDefaultCommands);
+  }
+
+  private void processDefaultCommands(Mechanism mechanism) {
+    var bindings = m_defaultCommandBindings.get(mechanism);
+
+    // Remove default command bindings that are no longer active.
+    // If a default command is running when its scope goes inactive, also be sure to cancel it.
+    bindings.removeIf(
+        b -> {
+          if (!b.scope().active()) {
+            cancel(b.command());
+            return true;
           }
+          return false;
         });
+
+    if (bindings.isEmpty()) {
+      // Nothing to do. No active bindings remain.
+      return;
+    }
+
+    // Cancel any default command except the narrowest-scoped one (the last binding in the list)
+    for (int i = 0; i < bindings.size() - 1; i++) {
+      Command widerScopeDefaultCommand = bindings.get(i).command();
+      cancel(widerScopeDefaultCommand);
+    }
+
+    // Check if the mechanism is currently in use. We can queue the default command if it's not.
+    for (Command runningCommand : m_runningCommands.keySet()) {
+      if (runningCommand.requires(mechanism)) {
+        return;
+      }
+    }
+    for (CommandState queuedState : m_queuedToRun) {
+      if (queuedState.command().requires(mechanism)) {
+        return;
+      }
+    }
+
+    // Nothing currently running or queued that needs this mechanism. Queue the default command.
+    var defaultCommand = bindings.getLast();
+    schedule(defaultCommand);
   }
 
   /**

commandsv3/src/main/java/org/wpilib/command3/Trigger.java

@@ -351,23 +351,7 @@ public class Trigger implements BooleanSupplier {
   }
 
   private void addBinding(BindingType bindingType, Command command) {
-    Command currentCommand = m_scheduler.currentCommand();
-    long currentOpmode = OpModeFetcher.getFetcher().getOpModeId();
-
-    BindingScope scope;
-    if (currentCommand != null) {
-      // A command is creating a binding - make it scoped to that specific command.
-      // The binding will be removed when the command exits.
-      scope = BindingScope.forCommand(m_scheduler, currentCommand);
-    } else if (currentOpmode != 0) {
-      // An opmode is currently running; scope the binding to just that mode.
-      // The binding will be removed when the opmode exits.
-      scope = BindingScope.forOpmode(currentOpmode);
-    } else {
-      // No opmode selected and no command is running; the binding is global in scope.
-      scope = BindingScope.global();
-    }
-
+    var scope = BindingScope.createNarrowestScope(m_scheduler);
     addBinding(scope, bindingType, command);
   }
 }

commandsv3/src/test/java/org/wpilib/command3/BindingScopeTest.java

@@ -0,0 +1,78 @@
+// 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 org.wpilib.command3;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertInstanceOf;
+import static org.junit.jupiter.api.Assertions.assertNotNull;
+import static org.junit.jupiter.api.Assertions.assertSame;
+
+import org.junit.jupiter.api.Test;
+
+class BindingScopeTest extends SchedulerTest {
+  @Test
+  void narrowestScopeInOpmode() {
+    m_opModeId = 1;
+    m_opModeName = "Opmode 1";
+
+    var scope = BindingScope.createNarrowestScope(m_scheduler);
+    assertInstanceOf(BindingScope.ForOpmode.class, scope);
+    assertEquals(m_opModeId, ((BindingScope.ForOpmode) scope).opmodeId());
+  }
+
+  @Test
+  void narrowestScopeInCommand() {
+    BindingScope[] scopeRef = new BindingScope[1];
+
+    var command =
+        Command.noRequirements()
+            .executing(
+                coroutine -> {
+                  BindingScope scope = BindingScope.createNarrowestScope(m_scheduler);
+                  scopeRef[0] = scope;
+                })
+            .named("Command 1");
+
+    m_scheduler.schedule(command);
+    m_scheduler.run();
+    assertNotNull(scopeRef[0], "Command did not execute");
+    assertInstanceOf(BindingScope.ForCommand.class, scopeRef[0]);
+    assertSame(command, ((BindingScope.ForCommand) scopeRef[0]).command());
+  }
+
+  @Test
+  void narrowestScopeInOpmodeAndCommand() {
+    m_opModeId = 314;
+    m_opModeName = "Opmode pi";
+
+    BindingScope[] scopeRef = new BindingScope[1];
+    var command =
+        Command.noRequirements()
+            .executing(
+                coroutine -> {
+                  BindingScope scope = BindingScope.createNarrowestScope(m_scheduler);
+                  scopeRef[0] = scope;
+                })
+            .named("Command pi");
+
+    m_scheduler.schedule(command);
+    m_scheduler.run();
+
+    // Scope should be for the command, not the opmode
+    assertNotNull(scopeRef[0], "Command did not execute");
+    assertInstanceOf(BindingScope.ForCommand.class, scopeRef[0]);
+    assertSame(command, ((BindingScope.ForCommand) scopeRef[0]).command());
+  }
+
+  @Test
+  void narrowestScopeWithoutOpmodeOrCommand() {
+    // Just to be explicit: an opmode ID of zero means no opmode is selected in the DS
+    m_opModeId = 0;
+    m_opModeName = null;
+
+    var scope = BindingScope.createNarrowestScope(m_scheduler);
+    assertSame(BindingScope.Global.INSTANCE, scope);
+  }
+}

commandsv3/src/test/java/org/wpilib/command3/CommandTestBase.java

@@ -6,12 +6,15 @@ package org.wpilib.command3;
 
 import java.util.ArrayList;
 import java.util.List;
+import org.junit.jupiter.api.AfterEach;
 import org.junit.jupiter.api.BeforeEach;
 import org.wpilib.system.RobotController;
 
 class CommandTestBase {
   protected Scheduler m_scheduler;
   protected List<SchedulerEvent> m_events;
+  protected long m_opModeId = 0;
+  protected String m_opModeName = "";
 
   @BeforeEach
   void initScheduler() {
@@ -20,4 +23,26 @@ class CommandTestBase {
     m_events = new ArrayList<>();
     m_scheduler.addEventListener(m_events::add);
   }
+
+  @BeforeEach
+  void initOpmodeFetcher() {
+    OpModeFetcher.setFetcher(
+        new OpModeFetcher() {
+          @Override
+          long getOpModeId() {
+            return m_opModeId;
+          }
+
+          @Override
+          String getOpModeName() {
+            return m_opModeName;
+          }
+        });
+  }
+
+  @AfterEach
+  void resetOpmodeFetcher() {
+    m_opModeId = 0;
+    m_opModeName = "";
+  }
 }

commandsv3/src/test/java/org/wpilib/command3/SchedulerCancellationTests.java

@@ -179,9 +179,14 @@ class SchedulerCancellationTests extends CommandTestBase {
 
     // Then running should get it into the set of running commands
     m_scheduler.run();
+    assertTrue(m_scheduler.isRunning(command));
+    for (Mechanism mechanism : mechanisms) {
+      assertEquals(List.of(command), m_scheduler.getRunningCommandsFor(mechanism));
+    }
 
     // Canceling should clear out the set of running commands
     m_scheduler.cancelAll();
+    assertFalse(m_scheduler.isRunning(command), "Command was not canceled by cancelAll()");
 
     // Then ticking the scheduler once to fully remove the command and schedule the defaults
     m_scheduler.run();

commandsv3/src/test/java/org/wpilib/command3/SchedulerDefaultCommandTests.java

@@ -0,0 +1,218 @@
+// 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 org.wpilib.command3;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import java.util.List;
+import org.junit.jupiter.api.Test;
+
+class SchedulerDefaultCommandTests extends CommandTestBase {
+  @Test
+  void globalDefaultCommandIsAlwaysUsed() {
+    var mech = new Mechanism("Mech", m_scheduler);
+    var defaultCommand = mech.run(Coroutine::park).named("Custom Default Command");
+
+    mech.setDefaultCommand(defaultCommand);
+    assertFalse(m_scheduler.isScheduledOrRunning(defaultCommand));
+
+    m_scheduler.run();
+    assertTrue(m_scheduler.isRunning(defaultCommand));
+
+    m_opModeId = 1;
+    m_opModeName = "Opmode 1";
+
+    m_scheduler.run();
+    assertTrue(
+        m_scheduler.isRunning(defaultCommand),
+        "Default command should still be running when the opmode changes");
+  }
+
+  @Test
+  void defaultCommandSetInOpmodeStops() {
+    var mech = new Mechanism("Mech", m_scheduler);
+    var initialDefaultCommand = mech.idle();
+    mech.setDefaultCommand(initialDefaultCommand);
+
+    m_opModeId = 1;
+    m_opModeName = "Opmode 1";
+
+    var opModeScopedCommand = mech.run(Coroutine::park).named("Custom Default Command");
+    mech.setDefaultCommand(opModeScopedCommand);
+
+    m_scheduler.run();
+    assertTrue(m_scheduler.isRunning(opModeScopedCommand));
+
+    // Change opmode. The globally scoped default command should run
+    m_opModeId = 2;
+    m_opModeName = "Opmode 2";
+    m_scheduler.run();
+    assertFalse(m_scheduler.isRunning(opModeScopedCommand));
+    assertTrue(m_scheduler.isRunning(initialDefaultCommand));
+  }
+
+  @Test
+  void defaultCommandSetInCommandStops() {
+    var mech = new Mechanism("Mech", m_scheduler);
+    var initialDefaultCommand = mech.idle();
+    mech.setDefaultCommand(initialDefaultCommand);
+
+    var commandScopedCommand = mech.run(Coroutine::park).named("Command-Scoped Default Command");
+
+    var scopingCommand =
+        Command.noRequirements()
+            .executing(
+                co -> {
+                  mech.setDefaultCommand(commandScopedCommand);
+                  co.park();
+                })
+            .named("Scoping Command");
+
+    m_scheduler.schedule(scopingCommand);
+    m_scheduler.run();
+    assertTrue(m_scheduler.isRunning(scopingCommand));
+    assertTrue(m_scheduler.isRunning(commandScopedCommand));
+    assertFalse(m_scheduler.isRunning(initialDefaultCommand));
+
+    m_scheduler.cancel(scopingCommand);
+    m_scheduler.run();
+    assertFalse(m_scheduler.isRunning(scopingCommand));
+    assertFalse(m_scheduler.isRunning(commandScopedCommand));
+    assertTrue(m_scheduler.isRunning(initialDefaultCommand));
+  }
+
+  @Test
+  void interruptingDefaultCommandInterruptsOwner() {
+    var mech = new Mechanism("Mech", m_scheduler);
+    var initialDefaultCommand = mech.idle();
+    mech.setDefaultCommand(initialDefaultCommand);
+
+    var commandScopedCommand = mech.run(Coroutine::park).named("Command-Scoped Default Command");
+    var scopingCommand =
+        Command.noRequirements()
+            .executing(
+                co -> {
+                  mech.setDefaultCommand(commandScopedCommand);
+                  co.park();
+                })
+            .named("Scoping Command");
+
+    final Command interruptor = mech.run(Coroutine::park).named("Interruptor");
+    m_scheduler.schedule(scopingCommand);
+    m_scheduler.run();
+    assertTrue(m_scheduler.isRunning(scopingCommand));
+    assertTrue(m_scheduler.isRunning(commandScopedCommand));
+    assertFalse(m_scheduler.isRunning(initialDefaultCommand));
+
+    m_scheduler.schedule(interruptor);
+    m_scheduler.run();
+    assertFalse(m_scheduler.isRunning(scopingCommand));
+    assertFalse(m_scheduler.isRunning(commandScopedCommand));
+    assertFalse(m_scheduler.isRunning(initialDefaultCommand));
+    assertTrue(m_scheduler.isRunning(interruptor));
+  }
+
+  @Test
+  void defaultCommandStackup() {
+    var mech = new Mechanism("Mech", m_scheduler);
+    var initialDefaultCommand = mech.idle();
+    mech.setDefaultCommand(initialDefaultCommand);
+
+    var opModeScopedCommand = mech.run(Coroutine::park).named("Opmode Scoped Default Command");
+    var commandScopedCommand = mech.run(Coroutine::park).named("Command-Scoped Default Command");
+
+    final Command scopingCommand =
+        Command.noRequirements()
+            .executing(
+                co -> {
+                  mech.setDefaultCommand(commandScopedCommand);
+                  co.park();
+                })
+            .named("Scoping Command");
+
+    m_opModeId = 1;
+    m_opModeName = "Opmode 1";
+    mech.setDefaultCommand(opModeScopedCommand);
+
+    m_scheduler.run();
+    assertTrue(
+        m_scheduler.isRunning(opModeScopedCommand),
+        "Opmode-scoped default command should be active");
+
+    m_scheduler.schedule(scopingCommand);
+    m_scheduler.run();
+    assertFalse(
+        m_scheduler.isRunning(opModeScopedCommand),
+        "Opmode-scoped default command should have stopped");
+    assertTrue(m_scheduler.isRunning(commandScopedCommand));
+
+    m_events.clear();
+    m_opModeId = 2;
+    m_opModeName = "Opmode 2";
+    m_scheduler.run();
+
+    assertFalse(
+        m_scheduler.isRunning(opModeScopedCommand),
+        "Opmode-scoped default command should have stopped when the opmode changed");
+    assertFalse(
+        m_scheduler.isRunning(scopingCommand),
+        "Running opmode-scoped command should have stopped when the opmode changed");
+    assertFalse(
+        m_scheduler.isRunning(commandScopedCommand),
+        "Command-scoped default command should have stopped when its parent command stopped");
+    assertTrue(
+        m_scheduler.isRunning(initialDefaultCommand), "Global default command should be active");
+  }
+
+  @Test
+  void defaultCommandChangingDefaultCommand() {
+    var mech =
+        new Mechanism("Mech", m_scheduler) {
+          Command makeCommand1() {
+            return run(co -> {
+                  setDefaultCommand(makeCommand2());
+                  // One-shot default commands won't appear in the `getRunningCommandsFor` list,
+                  // We need to park the coroutine so we can observe the change
+                  co.park();
+                })
+                .named("Command 1");
+          }
+
+          Command makeCommand2() {
+            return run(co -> {
+                  setDefaultCommand(makeCommand1());
+                  co.park();
+                })
+                .named("Command 2");
+          }
+        };
+
+    mech.setDefaultCommand(mech.makeCommand1());
+
+    // Command 1 and Command 2 should alternate as default commands
+    for (int i = 0; i < 10; i++) {
+      m_scheduler.run();
+
+      String expectedDefaultCommand = i % 2 == 0 ? "Command 1" : "Command 2";
+      assertEquals(
+          List.of(expectedDefaultCommand),
+          m_scheduler.getRunningCommandsFor(mech).stream().map(Command::name).toList());
+    }
+
+    // Check for memory leaks. If every call to `setDefaultCommand` were to add a new binding,
+    // we'd have 1 binding object per loop and quickly use a ton of memory
+    var defaultCommandBindings = m_scheduler.getDefaultCommandBindingsFor(mech);
+    assertEquals(
+        List.of("Mech[IDLE]", "Command 1", "Command 2", "Command 1"),
+        defaultCommandBindings.stream().map(b -> b.command().name()).toList());
+
+    m_scheduler.run();
+    assertEquals(
+        List.of("Mech[IDLE]", "Command 1", "Command 2"),
+        defaultCommandBindings.stream().map(b -> b.command().name()).toList());
+  }
+}