[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();
  }
}
```

wpilibc/src/main/native/cpp/driverstation/DriverStation.cpp

@@ -159,6 +159,7 @@ struct Instance {
   // Op mode lookup
   wpi::util::mutex opModeMutex;
   wpi::util::DenseMap<int64_t, HAL_OpModeOption> opModes;
+  bool userProgramStarted = false;
 
   wpi::units::second_t nextMessageTime = 0_s;
 
@@ -679,7 +680,24 @@ void DriverStation::ClearOpModes() {
   HAL_SetOpModeOptions(nullptr, 0);
 }
 
+void DriverStation::ObserveUserProgramStarting() {
+  ::GetInstance().userProgramStarted = true;
+  HAL_ObserveUserProgramStarting();
+}
+
+int64_t DriverStation::GetOpModeId() {
+  if (!::GetInstance().userProgramStarted) {
+    return 0;
+  }
+
+  return GetControlWord().GetOpModeId();
+}
+
 std::string DriverStation::GetOpMode() {
+  if (!::GetInstance().userProgramStarted) {
+    return 0;
+  }
+
   return GetInstance().OpModeToString(GetOpModeId());
 }
 

wpilibc/src/main/native/cpp/framework/OpModeRobot.cpp

@@ -117,13 +117,14 @@ void OpModeRobotBase::StartCompetition() {
     // Wait for new data from the driver station, with 50 ms timeout
     HAL_SetNotifierAlarm(m_notifier, 50000, 0, false, true, &status);
 
-    // Call HAL_ObserveUserProgramStarting() here as a one-shot to ensure it is
-    // called after the notifier alarm is set.  The notifier alarm is set using
-    // relative time, so tests that wait on the user program to start and then
-    // step time won't work correctly if we call this before setting the alarm.
+    // Call DriverStation::ObserveUserProgramStarting() here as a one-shot to
+    // ensure it is called after the notifier alarm is set.  The notifier alarm
+    // is set using relative time, so tests that wait on the user program to
+    // start and then step time won't work correctly if we call this before
+    // setting the alarm.
     if (!calledObserveUserProgramStarting) {
       calledObserveUserProgramStarting = true;
-      HAL_ObserveUserProgramStarting();
+      DriverStation::ObserveUserProgramStarting();
     }
 
     auto signaled = wpi::util::WaitForObjects(events, signaledBuf);

wpilibc/src/main/native/cpp/framework/TimedRobot.cpp

@@ -9,7 +9,7 @@
 #include <cstdio>
 #include <utility>
 
-#include "wpi/hal/DriverStation.h"
+#include "wpi/driverstation/DriverStation.hpp"
 #include "wpi/hal/Notifier.hpp"
 #include "wpi/hal/UsageReporting.hpp"
 #include "wpi/system/Errors.hpp"
@@ -23,7 +23,7 @@ void TimedRobot::StartCompetition() {
 
   // Tell the DS that the robot is ready to be enabled
   std::puts("\n********** Robot program startup complete **********");
-  HAL_ObserveUserProgramStarting();
+  DriverStation::ObserveUserProgramStarting();
 
   // Loop forever, calling the appropriate mode-dependent function
   while (true) {

wpilibc/src/main/native/include/wpi/driverstation/DriverStation.hpp

@@ -454,6 +454,20 @@ class DriverStation final {
    */
   static void ClearOpModes();
 
+  /**
+   * Sets the program starting flag in the DS. This will also allow
+   * getOpModeId() and getOpMode() to return values for the selected
+   * OpMode in the DS application, if the DS is connected by the time this
+   * method is called.
+   *
+   * <p>Most users will not need to use this method; the TimedRobot and
+   * OpModeRobot robot framework classes will call it automatically after
+   * the main robot class is instantiated.
+   *
+   * <p>This is what changes the DS to showing robot code ready.
+   */
+  static void ObserveUserProgramStarting();
+
   /**
    * Gets the operating mode selected on the driver station. Note this does not
    * mean the robot is enabled; use IsEnabled() for that. In a match, this will
@@ -464,7 +478,7 @@ class DriverStation final {
    * @return the unique ID provided by the AddOpMode() function; may return 0 or
    * a unique ID not added, so callers should be prepared to handle that case
    */
-  static int64_t GetOpModeId() { return GetControlWord().GetOpModeId(); }
+  static int64_t GetOpModeId();
 
   /**
    * Gets the operating mode selected on the driver station. Note this does not