[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>

wpilibcExamples/src/main/cpp/examples/HAL/c/Robot.c

@@ -15,36 +15,18 @@ that want even more control over what code runs on their robot.
 
 #include <stdio.h>
 
+#include "wpi/hal/DriverStationTypes.h"
 #include "wpi/hal/HAL.h"
 
-enum DriverStationMode {
-  DisabledMode,
-  TeleopMode,
-  TestMode,
-  AutoMode,
-};
-
-enum DriverStationMode getDSMode(void) {
+HAL_RobotMode getDSMode(void) {
   // Get Robot State
   HAL_ControlWord word;
   HAL_GetControlWord(&word);
 
   // We send the observes, otherwise the DS disables
-  if (!word.enabled) {
-    HAL_ObserveUserProgramDisabled();
-    return DisabledMode;
-  } else {
-    if (word.autonomous) {
-      HAL_ObserveUserProgramAutonomous();
-      return AutoMode;
-    } else if (word.test) {
-      HAL_ObserveUserProgramTest();
-      return TestMode;
-    } else {
-      HAL_ObserveUserProgramTeleop();
-      return TeleopMode;
-    }
-  }
+  HAL_ObserveUserProgram(word);
+  return HAL_ControlWord_IsEnabled(word) ? HAL_ControlWord_GetRobotMode(word)
+                                         : HAL_ROBOTMODE_UNKNOWN;
 }
 
 int main(void) {
@@ -57,6 +39,28 @@ int main(void) {
 
   int32_t status = 0;
 
+  // Create an opmode per robot mode
+  static struct HAL_OpModeOption opmodes[] = {
+      {HAL_MAKE_OPMODEID(HAL_ROBOTMODE_AUTONOMOUS, 0),
+       {"Auto", 4},
+       {"", 0},
+       {"", 0},
+       -1,
+       -1},
+      {HAL_MAKE_OPMODEID(HAL_ROBOTMODE_TELEOPERATED, 0),
+       {"Teleop", 6},
+       {"", 0},
+       {"", 0},
+       -1,
+       -1},
+      {HAL_MAKE_OPMODEID(HAL_ROBOTMODE_TEST, 0),
+       {"Test", 4},
+       {"", 0},
+       {"", 0},
+       -1,
+       -1}};
+  HAL_SetOpModeOptions(opmodes, sizeof(opmodes) / sizeof(opmodes[0]));
+
   // For DS to see valid robot code
   HAL_ObserveUserProgramStarting();
 
@@ -95,11 +99,11 @@ int main(void) {
 
     HAL_RefreshDSData();
 
-    enum DriverStationMode dsMode = getDSMode();
+    HAL_RobotMode dsMode = getDSMode();
     switch (dsMode) {
-      case DisabledMode:
+      case HAL_ROBOTMODE_UNKNOWN:
         break;
-      case TeleopMode:
+      case HAL_ROBOTMODE_TELEOPERATED:
         status = 0;
         if (HAL_GetDIO(dio, &status)) {
           HAL_SetPWMPulseTimeMicroseconds(pwmPort, 2000, &status);
@@ -107,9 +111,9 @@ int main(void) {
           HAL_SetPWMPulseTimeMicroseconds(pwmPort, 1500, &status);
         }
         break;
-      case AutoMode:
+      case HAL_ROBOTMODE_AUTONOMOUS:
         break;
-      case TestMode:
+      case HAL_ROBOTMODE_TEST:
         break;
       default:
         break;