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

wpilibj/src/main/java/org/wpilib/driverstation/DriverStation.java

@@ -16,6 +16,8 @@ import org.wpilib.datalog.FloatArrayLogEntry;
 import org.wpilib.datalog.IntegerArrayLogEntry;
 import org.wpilib.datalog.StringLogEntry;
 import org.wpilib.datalog.StructLogEntry;
+import org.wpilib.framework.OpModeRobot;
+import org.wpilib.framework.TimedRobot;
 import org.wpilib.hardware.hal.AllianceStationID;
 import org.wpilib.hardware.hal.ControlWord;
 import org.wpilib.hardware.hal.DriverStationJNI;
@@ -578,6 +580,7 @@ public final class DriverStation {
 
   private static boolean m_silenceJoystickWarning;
 
+  private static boolean m_userProgramStarted = false;
   private static final Map<Long, OpModeOption> m_opModes = new HashMap<>();
   private static final ReentrantLock m_opModesMutex = new ReentrantLock();
 
@@ -1469,16 +1472,47 @@ public final class DriverStation {
     }
   }
 
+  /**
+   * Sets the program starting flag in the DS. This will also allow {@link #getOpModeId()} and
+   * {@link #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 {@link TimedRobot} and {@link OpModeRobot}
+   * robot framework classes will call it automatically after the main robot class is instantiated.
+   * However, teams using the commandsv3 library and a custom main robot class need to be careful to
+   * only call this method after all mechanisms and global trigger bindings are set up. If not, any
+   * setup performed in the main robot class may be incorrectly bound to the opmode selected in the
+   * DS if it's connected by the time the robot program boots up.
+   *
+   * <p>This is what changes the DS to showing robot code ready.
+   *
+   * @see #getOpMode()
+   * @see #getOpModeId()
+   */
+  public static void observeUserProgramStarting() {
+    m_userProgramStarted = true;
+    DriverStationJNI.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 indicate the operating mode selected
    * for auto before the match starts (i.e., while the robot is disabled in auto mode); after the
    * auto period ends, this will change to reflect the operating mode selected for teleop.
    *
+   * <p>This method always returns {@code 0} while the main robot class is being constructed and
+   * initialized (more specifically, it returns {@code 0} until {@link
+   * #observeUserProgramStarting()} is called, which the WPILib framework will automatically call
+   * during {@link TimedRobot#startCompetition()} and {@link OpModeRobot#startCompetition()}).
+   *
    * @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
    */
   public static long getOpModeId() {
+    if (!m_userProgramStarted) {
+      return 0;
+    }
+
     m_cacheDataMutex.lock();
     try {
       return m_controlWord.getOpModeId();
@@ -1493,10 +1527,19 @@ public final class DriverStation {
    * for auto before the match starts (i.e., while the robot is disabled in auto mode); after the
    * auto period ends, this will change to reflect the operating mode selected for teleop.
    *
+   * <p>This method always returns an empty string {@code ""} while the main robot class is being
+   * constructed and initialized (more specifically, it returns {@code ""} until {@link
+   * #observeUserProgramStarting()} is called, which the WPILib framework will automatically call
+   * during {@link TimedRobot#startCompetition()} and {@link OpModeRobot#startCompetition()}).
+   *
    * @return Operating mode string; may return a string not in the list of options, so callers
    *     should be prepared to handle that case
    */
   public static String getOpMode() {
+    if (!m_userProgramStarted) {
+      return "";
+    }
+
     m_cacheDataMutex.lock();
     try {
       return m_opMode;

wpilibj/src/main/java/org/wpilib/framework/OpModeRobot.java

@@ -617,7 +617,7 @@ public abstract class OpModeRobot extends RobotBase {
         // this before setting the alarm.
         if (!calledObserveUserProgramStarting) {
           calledObserveUserProgramStarting = true;
-          DriverStationJNI.observeUserProgramStarting();
+          DriverStation.observeUserProgramStarting();
         }
 
         try {

wpilibj/src/main/java/org/wpilib/framework/TimedRobot.java

@@ -7,7 +7,7 @@ package org.wpilib.framework;
 import static org.wpilib.units.Units.Seconds;
 
 import java.util.PriorityQueue;
-import org.wpilib.hardware.hal.DriverStationJNI;
+import org.wpilib.driverstation.DriverStation;
 import org.wpilib.hardware.hal.HAL;
 import org.wpilib.hardware.hal.NotifierJNI;
 import org.wpilib.system.RobotController;
@@ -128,7 +128,7 @@ public class TimedRobot extends IterativeRobotBase {
 
     // Tell the DS that the robot is ready to be enabled
     System.out.println("********** Robot program startup complete **********");
-    DriverStationJNI.observeUserProgramStarting();
+    DriverStation.observeUserProgramStarting();
 
     // Loop forever, calling the appropriate mode-dependent function
     while (true) {