[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/example_projects.bzl

@@ -86,6 +86,7 @@ SNIPPETS_FOLDERS = [
 TEMPLATES_FOLDERS = [
     "commandv2",
     "commandv2skeleton",
+    "opmode",
     "robotbaseskeleton",
     "timed",
     "timedskeleton",

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;

wpilibcExamples/src/main/cpp/templates/opmode/cpp/Robot.cpp

@@ -0,0 +1,31 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "Robot.hpp"
+
+#include "opmode/MyAuto.hpp"
+#include "opmode/MyTeleop.hpp"
+
+Robot::Robot() {
+  // Add opmodes to the robot here.
+  AddOpMode<MyTeleop>(wpi::RobotMode::TELEOPERATED, "My Teleop", "",
+                      "An example teleop opmode");
+  AddOpMode<MyAuto>(wpi::RobotMode::AUTONOMOUS, "My Auto", "");
+  PublishOpModes();
+}
+
+/** This function is called exactly once when the DS first connects. */
+void Robot::DriverStationConnected() {}
+
+/**
+ * This function is called periodically anytime when no opmode is selected,
+ * including when the Driver Station is disconnected.
+ */
+void Robot::NonePeriodic() {}
+
+#ifndef RUNNING_WPILIB_TESTS
+int main() {
+  return wpi::StartRobot<Robot>();
+}
+#endif

wpilibcExamples/src/main/cpp/templates/opmode/cpp/opmode/MyAuto.cpp

@@ -0,0 +1,33 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "opmode/MyAuto.hpp"
+
+#include "Robot.hpp"
+
+/** The Robot instance is passed into the opmode via the constructor. */
+MyAuto::MyAuto(Robot& robot) : m_robot{robot} {
+  /*
+   * Can call the base class constructor with the period to set a different
+   * periodic time interval.
+   *
+   * Additional periodic methods may be configured with AddPeriodic().
+   */
+}
+
+MyAuto::~MyAuto() {
+  /* Called when the opmode is de-selected. */
+}
+
+void MyAuto::Start() {
+  /* Called once when the robot is first enabled. */
+}
+
+void MyAuto::Periodic() {
+  /* Called periodically (set time interval) while the robot is enabled. */
+}
+
+void MyAuto::End() {
+  /* Called when the robot is disabled (after previously being enabled). */
+}

wpilibcExamples/src/main/cpp/templates/opmode/cpp/opmode/MyTeleop.cpp

@@ -0,0 +1,26 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "opmode/MyTeleop.hpp"
+
+#include "Robot.hpp"
+
+/** The Robot instance is passed into the opmode via the constructor. */
+MyTeleop::MyTeleop(Robot& robot) : m_robot{robot} {}
+
+MyTeleop::~MyTeleop() {
+  /* Called when the opmode is de-selected. */
+}
+
+void MyTeleop::Start() {
+  /* Called once when the robot is first enabled. */
+}
+
+void MyTeleop::Periodic() {
+  /* Called periodically (set time interval) while the robot is enabled. */
+}
+
+void MyTeleop::End() {
+  /* Called when the robot is disabled (after previously being enabled). */
+}

wpilibcExamples/src/main/cpp/templates/opmode/include/Robot.hpp

@@ -0,0 +1,14 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "wpi/framework/OpModeRobot.hpp"
+
+class Robot : public wpi::OpModeRobot<Robot> {
+ public:
+  Robot();
+  void DriverStationConnected() override;
+  void NonePeriodic() override;
+};

wpilibcExamples/src/main/cpp/templates/opmode/include/opmode/MyAuto.hpp

@@ -0,0 +1,23 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "wpi/opmode/PeriodicOpMode.hpp"
+
+class Robot;
+
+class MyAuto : public wpi::PeriodicOpMode {
+ public:
+  /** The Robot instance is passed into the opmode via the constructor. */
+  explicit MyAuto(Robot& robot);
+  ~MyAuto() override;
+  void Start() override;
+  void Periodic() override;
+  void End() override;
+
+ private:
+  [[maybe_unused]]
+  Robot& m_robot;
+};

wpilibcExamples/src/main/cpp/templates/opmode/include/opmode/MyTeleop.hpp

@@ -0,0 +1,23 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "wpi/opmode/PeriodicOpMode.hpp"
+
+class Robot;
+
+class MyTeleop : public wpi::PeriodicOpMode {
+ public:
+  /** The Robot instance is passed into the opmode via the constructor. */
+  explicit MyTeleop(Robot& robot);
+  ~MyTeleop() override;
+  void Start() override;
+  void Periodic() override;
+  void End() override;
+
+ private:
+  [[maybe_unused]]
+  Robot& m_robot;
+};

wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/cpp/Robot.cpp

@@ -7,7 +7,6 @@
 #include "wpi/driverstation/DriverStation.hpp"
 #include "wpi/hal/DriverStation.h"
 #include "wpi/internal/DriverStationModeThread.hpp"
-#include "wpi/nt/NetworkTable.hpp"
 
 Robot::Robot() {}
 
@@ -20,7 +19,13 @@ void Robot::Teleop() {}
 void Robot::Test() {}
 
 void Robot::StartCompetition() {
-  wpi::internal::DriverStationModeThread modeThread;
+  wpi::internal::DriverStationModeThread modeThread{wpi::hal::GetControlWord()};
+
+  // Create an opmode per robot mode
+  wpi::DriverStation::AddOpMode(wpi::RobotMode::AUTONOMOUS, "Auto");
+  wpi::DriverStation::AddOpMode(wpi::RobotMode::TELEOPERATED, "Teleop");
+  wpi::DriverStation::AddOpMode(wpi::RobotMode::TEST, "Test");
+  wpi::DriverStation::PublishOpModes();
 
   wpi::util::Event event{false, false};
   wpi::DriverStation::ProvideRefreshedDataEventHandle(event.GetHandle());
@@ -29,31 +34,24 @@ void Robot::StartCompetition() {
   HAL_ObserveUserProgramStarting();
 
   while (!m_exit) {
+    modeThread.InControl(wpi::DriverStation::GetControlWord());
     if (IsDisabled()) {
-      modeThread.InDisabled(true);
       Disabled();
-      modeThread.InDisabled(false);
       while (IsDisabled()) {
         wpi::util::WaitForObject(event.GetHandle());
       }
     } else if (IsAutonomous()) {
-      modeThread.InAutonomous(true);
       Autonomous();
-      modeThread.InAutonomous(false);
       while (IsAutonomousEnabled()) {
         wpi::util::WaitForObject(event.GetHandle());
       }
     } else if (IsTest()) {
-      modeThread.InTest(true);
       Test();
-      modeThread.InTest(false);
       while (IsTest() && IsEnabled()) {
         wpi::util::WaitForObject(event.GetHandle());
       }
     } else {
-      modeThread.InTeleop(true);
       Teleop();
-      modeThread.InTeleop(false);
       while (IsTeleopEnabled()) {
         wpi::util::WaitForObject(event.GetHandle());
       }

wpilibcExamples/src/main/cpp/templates/templates.json

@@ -20,6 +20,16 @@
     "gradlebase": "cpp",
     "commandversion": 2
   },
+  {
+    "name": "OpMode Robot",
+    "description": "OpMode style, with explanatory comments and example code.",
+    "tags": [
+      "OpMode"
+    ],
+    "foldername": "opmode",
+    "gradlebase": "cpp",
+    "commandversion": 2
+  },
   {
     "name": "Timed Robot",
     "description": "Timed style, with explanatory comments and example code.",

wpilibcExamples/src/test/cpp/examples/ArmSimulation/cpp/ArmSimulationTest.cpp

@@ -9,6 +9,7 @@
 
 #include "Constants.hpp"
 #include "Robot.hpp"
+#include "wpi/hal/DriverStationTypes.h"
 #include "wpi/hal/simulation/MockHooks.h"
 #include "wpi/simulation/DriverStationSim.hpp"
 #include "wpi/simulation/JoystickSim.hpp"
@@ -57,7 +58,7 @@ TEST_P(ArmSimulationTest, Teleop) {
 
   // teleop init
   {
-    wpi::sim::DriverStationSim::SetAutonomous(false);
+    wpi::sim::DriverStationSim::SetRobotMode(HAL_ROBOTMODE_TELEOPERATED);
     wpi::sim::DriverStationSim::SetEnabled(true);
     wpi::sim::DriverStationSim::NotifyNewData();
 
@@ -130,7 +131,6 @@ TEST_P(ArmSimulationTest, Teleop) {
 
   {
     // Disable
-    wpi::sim::DriverStationSim::SetAutonomous(false);
     wpi::sim::DriverStationSim::SetEnabled(false);
     wpi::sim::DriverStationSim::NotifyNewData();
 

wpilibcExamples/src/test/cpp/examples/DigitalCommunication/cpp/DigitalCommunicationTest.cpp

@@ -8,6 +8,7 @@
 #include <gtest/gtest.h>
 
 #include "Robot.hpp"
+#include "wpi/hal/DriverStationTypes.h"
 #include "wpi/simulation/DIOSim.hpp"
 #include "wpi/simulation/DriverStationSim.hpp"
 #include "wpi/simulation/SimHooks.hpp"
@@ -120,7 +121,8 @@ class AutonomousTest : public DigitalCommunicationTest<bool> {};
 
 TEST_P(AutonomousTest, Autonomous) {
   auto autonomous = GetParam();
-  wpi::sim::DriverStationSim::SetAutonomous(autonomous);
+  wpi::sim::DriverStationSim::SetRobotMode(
+      autonomous ? HAL_ROBOTMODE_AUTONOMOUS : HAL_ROBOTMODE_TELEOPERATED);
   wpi::sim::DriverStationSim::NotifyNewData();
 
   EXPECT_TRUE(m_autonomousOutput.GetInitialized());

wpilibcExamples/src/test/cpp/examples/ElevatorSimulation/cpp/ElevatorSimulationTest.cpp

@@ -9,6 +9,7 @@
 
 #include "Constants.hpp"
 #include "Robot.hpp"
+#include "wpi/hal/DriverStationTypes.h"
 #include "wpi/hal/simulation/MockHooks.h"
 #include "wpi/simulation/DriverStationSim.hpp"
 #include "wpi/simulation/JoystickSim.hpp"
@@ -51,7 +52,7 @@ class ElevatorSimulationTest : public testing::Test {
 TEST_F(ElevatorSimulationTest, Teleop) {
   // teleop init
   {
-    wpi::sim::DriverStationSim::SetAutonomous(false);
+    wpi::sim::DriverStationSim::SetRobotMode(HAL_ROBOTMODE_TELEOPERATED);
     wpi::sim::DriverStationSim::SetEnabled(true);
     wpi::sim::DriverStationSim::NotifyNewData();
 
@@ -111,7 +112,6 @@ TEST_F(ElevatorSimulationTest, Teleop) {
 
   {
     // Disable
-    wpi::sim::DriverStationSim::SetAutonomous(false);
     wpi::sim::DriverStationSim::SetEnabled(false);
     wpi::sim::DriverStationSim::NotifyNewData();
 

wpilibcExamples/src/test/cpp/examples/I2CCommunication/cpp/I2CCommunicationTest.cpp

@@ -8,6 +8,7 @@
 #include <gtest/gtest.h>
 
 #include "Robot.hpp"
+#include "wpi/hal/DriverStationTypes.h"
 #include "wpi/hal/simulation/I2CData.h"
 #include "wpi/simulation/DriverStationSim.hpp"
 #include "wpi/simulation/SimHooks.hpp"
@@ -130,7 +131,8 @@ class AutonomousTest : public I2CCommunicationTest<bool> {};
 
 TEST_P(AutonomousTest, Autonomous) {
   auto autonomous = GetParam();
-  wpi::sim::DriverStationSim::SetAutonomous(autonomous);
+  wpi::sim::DriverStationSim::SetRobotMode(
+      autonomous ? HAL_ROBOTMODE_AUTONOMOUS : HAL_ROBOTMODE_TELEOPERATED);
   wpi::sim::DriverStationSim::NotifyNewData();
 
   EXPECT_TRUE(HALSIM_GetI2CInitialized(m_port));

wpilibcExamples/src/test/cpp/examples/PotentiometerPID/cpp/PotentiometerPIDTest.cpp

@@ -8,6 +8,7 @@
 #include <gtest/gtest.h>
 
 #include "Robot.hpp"
+#include "wpi/hal/DriverStationTypes.h"
 #include "wpi/hal/simulation/MockHooks.h"
 #include "wpi/math/system/plant/DCMotor.hpp"
 #include "wpi/simulation/AnalogInputSim.hpp"
@@ -92,7 +93,7 @@ class PotentiometerPIDTest : public testing::Test {
 TEST_F(PotentiometerPIDTest, Teleop) {
   // teleop init
   {
-    wpi::sim::DriverStationSim::SetAutonomous(false);
+    wpi::sim::DriverStationSim::SetRobotMode(HAL_ROBOTMODE_TELEOPERATED);
     wpi::sim::DriverStationSim::SetEnabled(true);
     wpi::sim::DriverStationSim::NotifyNewData();