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

wpilibc/src/main/python/wpilib/__init__.py

@@ -16,7 +16,6 @@ from ._wpilib import (
     CANStatus,
     Compressor,
     CompressorConfigType,
-    DSControlWord,
     DataLogManager,
     DigitalInput,
     DigitalOutput,
@@ -37,6 +36,7 @@ from ._wpilib import (
     Joystick,
     Koors40,
     LEDPattern,
+    LinearOpMode,
     Mechanism2d,
     MechanismLigament2d,
     MechanismObject2d,
@@ -45,6 +45,9 @@ from ._wpilib import (
     MotorSafety,
     Notifier,
     OnboardIMU,
+    OpMode,
+    OpModeRobotBase,
+    PeriodicOpMode,
     PS4Controller,
     PS5Controller,
     PWM,
@@ -63,7 +66,6 @@ from ._wpilib import (
     Preferences,
     RobotBase,
     RobotController,
-    RobotState,
     RuntimeType,
     SendableBuilderImpl,
     SendableChooser,
@@ -106,7 +108,6 @@ __all__ = [
     "CANStatus",
     "Compressor",
     "CompressorConfigType",
-    "DSControlWord",
     "DataLogManager",
     "DigitalInput",
     "DigitalOutput",
@@ -127,6 +128,7 @@ __all__ = [
     "Joystick",
     "Koors40",
     "LEDPattern",
+    "LinearOpMode",
     "Mechanism2d",
     "MechanismLigament2d",
     "MechanismObject2d",
@@ -135,6 +137,9 @@ __all__ = [
     "MotorSafety",
     "Notifier",
     "OnboardIMU",
+    "OpMode",
+    "OpModeRobotBase",
+    "PeriodicOpMode",
     "PS4Controller",
     "PS5Controller",
     "PWM",
@@ -153,7 +158,6 @@ __all__ = [
     "Preferences",
     "RobotBase",
     "RobotController",
-    "RobotState",
     "RuntimeType",
     "SendableBuilderImpl",
     "SendableChooser",
@@ -191,6 +195,10 @@ __all__ += ["reportError", "reportWarning"]
 
 del _init__wpilib
 
+from .opmoderobot import OpModeRobot
+
+__all__ += ["OpModeRobot"]
+
 from .cameraserver import CameraServer
 from .deployinfo import getDeployData
 

wpilibc/src/main/python/wpilib/opmoderobot.py

@@ -0,0 +1,61 @@
+from hal import RobotMode
+from typing import Optional
+from wpiutil import Color
+
+__all__ = ["OpModeRobot"]
+
+from ._wpilib import OpModeRobotBase, OpMode
+
+class OpModeRobot(OpModeRobotBase):
+    """
+    OpModeRobot implements the opmode-based robot program framework.
+
+    The OpModeRobot class is intended to be subclassed by a user creating a robot
+    program.
+
+    Opmodes are constructed when selected on the driver station, and destroyed
+    when the robot is disabled after being enabled or a different opmode is
+    selected. When no opmode is selected, nonePeriodic() is called. The
+    driverStationConnected() function is called the first time the driver station
+    connects to the robot.
+    """
+
+    def __init__(self):
+        super().__init__()
+
+    def addOpMode(self,
+                  opmodeCls: type,
+                  mode: RobotMode,
+                  name: str,
+                  group: Optional[str] = None,
+                  description: Optional[str] = None,
+                  textColor: Optional[Color] = None,
+                  backgroundColor: Optional[Color] = None) -> None:
+        """
+        Adds an operating mode option. It's necessary to call PublishOpModes() to
+        make the added modes visible to the driver station.
+
+        The textColor and backgroundColor parameters are optional, but setting
+        only one has no effect (if only one is provided, it will be ignored).
+
+        :param opmodeCls: opmode class; must be a public, non-abstract subclass of OpMode
+                          with a constructor that either takes no arguments or accepts a
+                          single argument of this class's type (the latter is preferred).
+        :param mode: robot mode
+        :param name: name of the operating mode
+        :param group: group of the operating mode
+        :param description: description of the operating mode
+        :param textColor: text color
+        :param backgroundColor: background color
+        """
+        def makeOpModeInstance() -> OpMode:
+            # Try to instantiate with robot argument first
+            try:
+                return opmodeCls(self)  # type: ignore
+            except TypeError:
+                # Fallback to no-argument constructor
+                return opmodeCls()  # type: ignore
+        if textColor is None or backgroundColor is None:
+            self.addOpModeFactory(makeOpModeInstance, mode, name, group or "", description or "")
+        else:
+            self.addOpModeFactory(makeOpModeInstance, mode, name, group or "", description or "", textColor, backgroundColor)

wpilibc/src/main/python/wpilib/src/rpy/ControlWord.cpp

@@ -1,18 +0,0 @@
-#include "rpy/ControlWord.h"
-
-#include "wpi/hal/DriverStation.h"
-
-namespace rpy {
-
-std::tuple<bool, bool, bool> GetControlState() {
-  HAL_ControlWord controlWord;
-  HAL_GetControlWord(&controlWord);
-
-  bool enable = controlWord.enabled != 0 && controlWord.dsAttached != 0;
-  bool auton = controlWord.autonomous != 0;
-  bool test = controlWord.test != 0;
-
-  return std::make_tuple(enable, auton, test);
-}
-
-} // namespace rpy

wpilibc/src/main/python/wpilib/src/rpy/ControlWord.h

@@ -1,7 +0,0 @@
-#include <tuple>
-
-namespace rpy {
-
-std::tuple<bool, bool, bool> GetControlState();
-
-} // namespace rpy