[hal, wpilib] Add OpMode support (#7744)

User code:
- OpModeRobot used as the robot base class
- LinearOpMode and PeriodicOpMode are provided opmode base classes
- In Java, annotations can be used to automatically register opmode classes

Additional user code functionality:
- OpMode (string) is available in addition to the overall
auto/teleop/test robot mode
- OpMode does not indicate enable (enable/disable is still separate)
- The HAL API uses integer UIDs; these are exposed at the user API level
as well for faster checks
- User code creates opmodes on startup (these have name, category,
description, etc).

DS:
- DS will present opmode selection lists for auto and teleop for
match/practice. During a match, the DS will automatically activate the
selected opmode in the corresponding match period.
- For testing, an overall mode is selected (e.g. teleop/auto/test) and a
single opmode is selected

Future work:
- Command framework support/integration
- Python annotation support
- Unit tests (needs race-free DS sim updates)
- Porting of examples

Co-authored-by: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com>

commandsv2/src/main/java/org/wpilib/command2/CommandScheduler.java

@@ -26,7 +26,6 @@ import org.wpilib.command2.Command.InterruptionBehavior;
 import org.wpilib.driverstation.DriverStation;
 import org.wpilib.event.EventLoop;
 import org.wpilib.framework.RobotBase;
-import org.wpilib.framework.RobotState;
 import org.wpilib.framework.TimedRobot;
 import org.wpilib.hardware.hal.HAL;
 import org.wpilib.system.Watchdog;
@@ -190,7 +189,7 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
     // run when disabled, or the command is already scheduled.
     if (m_disabled
         || isScheduled(command)
-        || RobotState.isDisabled() && !command.runsWhenDisabled()) {
+        || DriverStation.isDisabled() && !command.runsWhenDisabled()) {
       return;
     }
 
@@ -270,7 +269,7 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
     m_watchdog.addEpoch("buttons.run()");
 
     m_inRunLoop = true;
-    boolean isDisabled = RobotState.isDisabled();
+    boolean isDisabled = DriverStation.isDisabled();
     // Run scheduled commands, remove finished commands.
     for (Iterator<Command> iterator = m_scheduledCommands.iterator(); iterator.hasNext(); ) {
       Command command = iterator.next();

commandsv2/src/main/native/cpp/frc2/command/CommandScheduler.cpp

@@ -12,8 +12,8 @@
 
 #include "wpi/commands2/CommandPtr.hpp"
 #include "wpi/commands2/Subsystem.hpp"
+#include "wpi/driverstation/DriverStation.hpp"
 #include "wpi/framework/RobotBase.hpp"
-#include "wpi/framework/RobotState.hpp"
 #include "wpi/framework/TimedRobot.hpp"
 #include "wpi/hal/HALBase.h"
 #include "wpi/hal/UsageReporting.h"
@@ -102,7 +102,7 @@ void CommandScheduler::Schedule(Command* command) {
   RequireUngrouped(command);
 
   if (m_impl->disabled || m_impl->scheduledCommands.contains(command) ||
-      (wpi::RobotState::IsDisabled() && !command->RunsWhenDisabled())) {
+      (wpi::DriverStation::IsDisabled() && !command->RunsWhenDisabled())) {
     return;
   }
 
@@ -184,7 +184,7 @@ void CommandScheduler::Run() {
   loopCache->Poll();
   m_watchdog.AddEpoch("buttons.Run()");
 
-  bool isDisabled = wpi::RobotState::IsDisabled();
+  bool isDisabled = wpi::DriverStation::IsDisabled();
   // create a new set to avoid iterator invalidation.
   for (Command* command : wpi::util::SmallSet(m_impl->scheduledCommands)) {
     if (!IsScheduled(command)) {

commandsv2/src/test/java/org/wpilib/MockHardwareExtension.java

@@ -8,6 +8,7 @@ import org.junit.jupiter.api.extension.BeforeAllCallback;
 import org.junit.jupiter.api.extension.ExtensionContext;
 import org.junit.jupiter.api.extension.ExtensionContext.Namespace;
 import org.wpilib.hardware.hal.HAL;
+import org.wpilib.hardware.hal.RobotMode;
 import org.wpilib.simulation.DriverStationSim;
 
 public final class MockHardwareExtension implements BeforeAllCallback {
@@ -31,9 +32,8 @@ public final class MockHardwareExtension implements BeforeAllCallback {
   private void initializeHardware() {
     HAL.initialize(500, 0);
     DriverStationSim.setDsAttached(true);
-    DriverStationSim.setAutonomous(false);
     DriverStationSim.setEnabled(true);
-    DriverStationSim.setTest(true);
+    DriverStationSim.setRobotMode(RobotMode.TEST);
     DriverStationSim.notifyNewData();
   }
 }

commandsv2/src/test/java/org/wpilib/command2/button/RobotModeTriggersTest.java

@@ -8,14 +8,14 @@ import static org.junit.jupiter.api.Assertions.assertTrue;
 
 import org.junit.jupiter.api.Test;
 import org.wpilib.command2.CommandTestBase;
+import org.wpilib.hardware.hal.RobotMode;
 import org.wpilib.simulation.DriverStationSim;
 
 class RobotModeTriggersTest extends CommandTestBase {
   @Test
   void autonomousTest() {
     DriverStationSim.resetData();
-    DriverStationSim.setAutonomous(true);
-    DriverStationSim.setTest(false);
+    DriverStationSim.setRobotMode(RobotMode.AUTONOMOUS);
     DriverStationSim.setEnabled(true);
     DriverStationSim.notifyNewData();
     Trigger auto = RobotModeTriggers.autonomous();
@@ -25,8 +25,7 @@ class RobotModeTriggersTest extends CommandTestBase {
   @Test
   void teleopTest() {
     DriverStationSim.resetData();
-    DriverStationSim.setAutonomous(false);
-    DriverStationSim.setTest(false);
+    DriverStationSim.setRobotMode(RobotMode.TELEOPERATED);
     DriverStationSim.setEnabled(true);
     DriverStationSim.notifyNewData();
     Trigger teleop = RobotModeTriggers.teleop();
@@ -36,8 +35,7 @@ class RobotModeTriggersTest extends CommandTestBase {
   @Test
   void testModeTest() {
     DriverStationSim.resetData();
-    DriverStationSim.setAutonomous(false);
-    DriverStationSim.setTest(true);
+    DriverStationSim.setRobotMode(RobotMode.TEST);
     DriverStationSim.setEnabled(true);
     DriverStationSim.notifyNewData();
     Trigger test = RobotModeTriggers.test();
@@ -47,8 +45,6 @@ class RobotModeTriggersTest extends CommandTestBase {
   @Test
   void disabledTest() {
     DriverStationSim.resetData();
-    DriverStationSim.setAutonomous(false);
-    DriverStationSim.setTest(false);
     DriverStationSim.setEnabled(false);
     DriverStationSim.notifyNewData();
     Trigger disabled = RobotModeTriggers.disabled();

commandsv2/src/test/native/cpp/wpi/command/button/RobotModeTriggersTest.cpp

@@ -7,6 +7,7 @@
 #include "../CommandTestBase.hpp"
 #include "wpi/commands2/button/Trigger.hpp"
 #include "wpi/driverstation/DriverStation.hpp"
+#include "wpi/hal/DriverStationTypes.h"
 #include "wpi/simulation/DriverStationSim.hpp"
 
 using namespace wpi::cmd;
@@ -15,8 +16,7 @@ class RobotModeTriggersTest : public CommandTestBase {};
 
 TEST(RobotModeTriggersTest, Autonomous) {
   DriverStationSim::ResetData();
-  DriverStationSim::SetAutonomous(true);
-  DriverStationSim::SetTest(false);
+  DriverStationSim::SetRobotMode(HAL_ROBOTMODE_AUTONOMOUS);
   DriverStationSim::SetEnabled(true);
   DriverStationSim::NotifyNewData();
   Trigger autonomous = RobotModeTriggers::Autonomous();
@@ -25,8 +25,7 @@ TEST(RobotModeTriggersTest, Autonomous) {
 
 TEST(RobotModeTriggersTest, Teleop) {
   DriverStationSim::ResetData();
-  DriverStationSim::SetAutonomous(false);
-  DriverStationSim::SetTest(false);
+  DriverStationSim::SetRobotMode(HAL_ROBOTMODE_TELEOPERATED);
   DriverStationSim::SetEnabled(true);
   DriverStationSim::NotifyNewData();
   Trigger teleop = RobotModeTriggers::Teleop();
@@ -35,8 +34,6 @@ TEST(RobotModeTriggersTest, Teleop) {
 
 TEST(RobotModeTriggersTest, Disabled) {
   DriverStationSim::ResetData();
-  DriverStationSim::SetAutonomous(false);
-  DriverStationSim::SetTest(false);
   DriverStationSim::SetEnabled(false);
   DriverStationSim::NotifyNewData();
   Trigger disabled = RobotModeTriggers::Disabled();
@@ -45,8 +42,7 @@ TEST(RobotModeTriggersTest, Disabled) {
 
 TEST(RobotModeTriggersTest, TestMode) {
   DriverStationSim::ResetData();
-  DriverStationSim::SetAutonomous(false);
-  DriverStationSim::SetTest(true);
+  DriverStationSim::SetRobotMode(HAL_ROBOTMODE_TEST);
   DriverStationSim::SetEnabled(true);
   DriverStationSim::NotifyNewData();
   Trigger test = RobotModeTriggers::Test();