[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/native/cpp/driverstation/DSControlWord.cpp

@@ -1,55 +0,0 @@
-// 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 "wpi/driverstation/DSControlWord.hpp"
-
-#include "wpi/hal/DriverStation.h"
-
-using namespace wpi;
-
-DSControlWord::DSControlWord() {
-  HAL_GetControlWord(&m_controlWord);
-}
-
-bool DSControlWord::IsEnabled() const {
-  return m_controlWord.enabled && m_controlWord.dsAttached;
-}
-
-bool DSControlWord::IsDisabled() const {
-  return !(m_controlWord.enabled && m_controlWord.dsAttached);
-}
-
-bool DSControlWord::IsEStopped() const {
-  return m_controlWord.eStop;
-}
-
-bool DSControlWord::IsAutonomous() const {
-  return m_controlWord.autonomous;
-}
-
-bool DSControlWord::IsAutonomousEnabled() const {
-  return m_controlWord.autonomous && m_controlWord.enabled &&
-         m_controlWord.dsAttached;
-}
-
-bool DSControlWord::IsTeleop() const {
-  return !(m_controlWord.autonomous || m_controlWord.test);
-}
-
-bool DSControlWord::IsTeleopEnabled() const {
-  return !m_controlWord.autonomous && !m_controlWord.test &&
-         m_controlWord.enabled && m_controlWord.dsAttached;
-}
-
-bool DSControlWord::IsTest() const {
-  return m_controlWord.test;
-}
-
-bool DSControlWord::IsDSAttached() const {
-  return m_controlWord.dsAttached;
-}
-
-bool DSControlWord::IsFMSAttached() const {
-  return m_controlWord.fmsAttached;
-}

wpilibc/src/main/native/cpp/driverstation/DriverStation.cpp

@@ -8,10 +8,12 @@
 
 #include <array>
 #include <atomic>
+#include <functional>
 #include <memory>
 #include <span>
 #include <string>
 #include <string_view>
+#include <vector>
 
 #include <fmt/format.h>
 
@@ -25,12 +27,18 @@
 #include "wpi/nt/NetworkTable.hpp"
 #include "wpi/nt/NetworkTableInstance.hpp"
 #include "wpi/nt/StringTopic.hpp"
+#include "wpi/nt/StructTopic.hpp"
 #include "wpi/system/Errors.hpp"
 #include "wpi/system/Timer.hpp"
+#include "wpi/util/Color.hpp"
+#include "wpi/util/DenseMap.hpp"
 #include "wpi/util/EventVector.hpp"
+#include "wpi/util/StringExtras.hpp"
 #include "wpi/util/condition_variable.hpp"
 #include "wpi/util/json.hpp"
 #include "wpi/util/mutex.hpp"
+#include "wpi/util/string.h"
+#include "wpi/util/struct/Struct.hpp"
 #include "wpi/util/timestamp.h"
 
 using namespace wpi;
@@ -70,6 +78,14 @@ class MatchDataSenderEntry {
 static constexpr std::string_view kSmartDashboardType = "FMSInfo";
 
 struct MatchDataSender {
+  MatchDataSender()
+      : controlWord{table->GetStructTopic<wpi::hal::ControlWord>("ControlWord")
+                        .Publish()},
+        opMode{table->GetStringTopic("OpMode").Publish()} {
+    controlWord.Set(prevControlWord);
+    opMode.Set("");
+  }
+
   std::shared_ptr<wpi::nt::NetworkTable> table =
       wpi::nt::NetworkTableInstance::GetDefault().GetTable("FMSInfo");
   MatchDataSenderEntry<wpi::nt::StringTopic> typeMetaData{
@@ -89,8 +105,9 @@ struct MatchDataSender {
                                                        true};
   MatchDataSenderEntry<wpi::nt::IntegerTopic> station{table, "StationNumber",
                                                       1};
-  MatchDataSenderEntry<wpi::nt::IntegerTopic> controlWord{table,
-                                                          "FMSControlData", 0};
+  wpi::nt::StructPublisher<wpi::hal::ControlWord> controlWord;
+  wpi::nt::StringPublisher opMode;
+  wpi::hal::ControlWord prevControlWord;
 };
 
 class JoystickLogSender {
@@ -119,11 +136,9 @@ class DataLogSender {
  private:
   std::atomic_bool m_initialized{false};
 
-  HAL_ControlWord m_prevControlWord;
-  wpi::log::BooleanLogEntry m_logEnabled;
-  wpi::log::BooleanLogEntry m_logAutonomous;
-  wpi::log::BooleanLogEntry m_logTest;
-  wpi::log::BooleanLogEntry m_logEstop;
+  hal::ControlWord m_prevControlWord;
+  wpi::log::StructLogEntry<hal::ControlWord> m_logControlWord;
+  wpi::log::StringLogEntry m_logOpMode;
 
   bool m_logJoysticks;
   std::array<JoystickLogSender, DriverStation::kJoystickPorts> m_joysticks;
@@ -146,13 +161,23 @@ struct Instance {
 
   bool silenceJoystickWarning = false;
 
-  // Robot state status variables
-  bool userInDisabled = false;
-  bool userInAutonomous = false;
-  bool userInTeleop = false;
-  bool userInTest = false;
+  // Op mode lookup
+  wpi::util::mutex opModeMutex;
+  wpi::util::DenseMap<int64_t, HAL_OpModeOption> opModes;
 
   wpi::units::second_t nextMessageTime = 0_s;
+
+  std::string OpModeToString(int64_t id) {
+    std::scoped_lock lock{opModeMutex};
+    if (id == 0) {
+      return "";
+    }
+    auto it = opModes.find(id);
+    if (it != opModes.end()) {
+      return std::string{wpi::util::to_string_view(&it->second.name)};
+    }
+    return fmt::format("<{}>", id);
+  }
 };
 }  // namespace
 
@@ -577,70 +602,103 @@ bool DriverStation::IsJoystickConnected(int stick) {
          GetStickPOVsAvailable(stick) != 0;
 }
 
-bool DriverStation::IsEnabled() {
-  HAL_ControlWord controlWord;
-  HAL_GetControlWord(&controlWord);
-  return controlWord.enabled && controlWord.dsAttached;
+static int64_t DoAddOpMode(RobotMode mode, std::string_view name,
+                           std::string_view group, std::string_view description,
+                           int32_t textColor, int32_t backgroundColor) {
+  if (wpi::util::trim(name).empty()) {
+    return 0;
+  }
+
+  WPI_String nameWpi = wpi::util::make_string(name);
+  WPI_String groupWpi = wpi::util::make_string(group);
+  WPI_String descriptionWpi = wpi::util::make_string(description);
+
+  auto& inst = ::GetInstance();
+  std::scoped_lock lock{inst.opModeMutex};
+  std::string nameCopy{name};
+  for (;;) {
+    int64_t id = HAL_MakeOpModeId(static_cast<HAL_RobotMode>(mode),
+                                  std::hash<std::string_view>{}(nameCopy));
+    auto [it, isNew] = inst.opModes.try_emplace(
+        id, HAL_OpModeOption{id, nameWpi, groupWpi, descriptionWpi, textColor,
+                             backgroundColor});
+    if (isNew) {
+      return id;
+    }
+    if (HAL_OpMode_GetRobotMode(it->second.id) ==
+            static_cast<HAL_RobotMode>(mode) &&
+        wpi::util::to_string_view(&it->second.name) == name) {
+      return 0;  // can't insert duplicate name
+    }
+    // collision, try again with space appended
+    nameCopy += ' ';
+  }
 }
 
-bool DriverStation::IsDisabled() {
-  HAL_ControlWord controlWord;
-  HAL_GetControlWord(&controlWord);
-  return !(controlWord.enabled && controlWord.dsAttached);
+static int32_t ConvertColorToInt(const wpi::util::Color& color) {
+  return ((static_cast<int32_t>(color.red * 255) & 0xff) << 16) |
+         ((static_cast<int32_t>(color.green * 255) & 0xff) << 8) |
+         (static_cast<int32_t>(color.blue * 255) & 0xff);
 }
 
-bool DriverStation::IsEStopped() {
-  HAL_ControlWord controlWord;
-  HAL_GetControlWord(&controlWord);
-  return controlWord.eStop;
+int64_t DriverStation::AddOpMode(RobotMode mode, std::string_view name,
+                                 std::string_view group,
+                                 std::string_view description,
+                                 const wpi::util::Color& textColor,
+                                 const wpi::util::Color& backgroundColor) {
+  return DoAddOpMode(mode, name, group, description,
+                     ConvertColorToInt(textColor),
+                     ConvertColorToInt(backgroundColor));
 }
 
-bool DriverStation::IsAutonomous() {
-  HAL_ControlWord controlWord;
-  HAL_GetControlWord(&controlWord);
-  return controlWord.autonomous;
+int64_t DriverStation::AddOpMode(RobotMode mode, std::string_view name,
+                                 std::string_view group,
+                                 std::string_view description) {
+  return DoAddOpMode(mode, name, group, description, -1, -1);
 }
 
-bool DriverStation::IsAutonomousEnabled() {
-  HAL_ControlWord controlWord;
-  HAL_GetControlWord(&controlWord);
-  return controlWord.autonomous && controlWord.enabled;
+int64_t DriverStation::RemoveOpMode(RobotMode mode, std::string_view name) {
+  if (wpi::util::trim(name).empty()) {
+    return 0;
+  }
+
+  auto& inst = ::GetInstance();
+  std::scoped_lock lock{inst.opModeMutex};
+  // we have to loop over all entries to find the one with the correct name
+  // because the of the unique ID generation scheme
+  for (auto it = inst.opModes.begin(), end = inst.opModes.end(); it != end;
+       ++it) {
+    if (HAL_OpMode_GetRobotMode(it->second.id) ==
+            static_cast<HAL_RobotMode>(mode) &&
+        wpi::util::to_string_view(&it->second.name) == name) {
+      int64_t id = it->second.id;
+      inst.opModes.erase(it);
+      return id;
+    }
+  }
+  return 0;
 }
 
-bool DriverStation::IsTeleop() {
-  HAL_ControlWord controlWord;
-  HAL_GetControlWord(&controlWord);
-  return !(controlWord.autonomous || controlWord.test);
+void DriverStation::PublishOpModes() {
+  auto& inst = ::GetInstance();
+  std::scoped_lock lock{inst.opModeMutex};
+  std::vector<HAL_OpModeOption> options;
+  options.reserve(inst.opModes.size());
+  for (auto&& [id, option] : inst.opModes) {
+    options.emplace_back(option);
+  }
+  HAL_SetOpModeOptions(options.data(), options.size());
 }
 
-bool DriverStation::IsTeleopEnabled() {
-  HAL_ControlWord controlWord;
-  HAL_GetControlWord(&controlWord);
-  return !controlWord.autonomous && !controlWord.test && controlWord.enabled;
+void DriverStation::ClearOpModes() {
+  auto& inst = ::GetInstance();
+  std::scoped_lock lock{inst.opModeMutex};
+  inst.opModes.clear();
+  HAL_SetOpModeOptions(nullptr, 0);
 }
 
-bool DriverStation::IsTest() {
-  HAL_ControlWord controlWord;
-  HAL_GetControlWord(&controlWord);
-  return controlWord.test;
-}
-
-bool DriverStation::IsTestEnabled() {
-  HAL_ControlWord controlWord;
-  HAL_GetControlWord(&controlWord);
-  return controlWord.test && controlWord.enabled;
-}
-
-bool DriverStation::IsDSAttached() {
-  HAL_ControlWord controlWord;
-  HAL_GetControlWord(&controlWord);
-  return controlWord.dsAttached;
-}
-
-bool DriverStation::IsFMSAttached() {
-  HAL_ControlWord controlWord;
-  HAL_GetControlWord(&controlWord);
-  return controlWord.fmsAttached;
+std::string DriverStation::GetOpMode() {
+  return GetInstance().OpModeToString(GetOpModeId());
 }
 
 std::string DriverStation::GetGameSpecificMessage() {
@@ -853,11 +911,16 @@ void SendMatchData() {
   inst.matchDataSender.replayNumber.Set(tmpDataStore.replayNumber);
   inst.matchDataSender.matchType.Set(static_cast<int>(tmpDataStore.matchType));
 
-  HAL_ControlWord ctlWord;
-  HAL_GetControlWord(&ctlWord);
-  int32_t wordInt = 0;
-  std::memcpy(&wordInt, &ctlWord, sizeof(wordInt));
-  inst.matchDataSender.controlWord.Set(wordInt);
+  hal::ControlWord ctlWord = hal::GetControlWord();
+  if (ctlWord != inst.matchDataSender.prevControlWord) {
+    int64_t opModeId = ctlWord.GetOpModeId();
+    if (opModeId != inst.matchDataSender.prevControlWord.GetOpModeId()) {
+      inst.matchDataSender.opMode.Set(inst.OpModeToString(opModeId));
+    }
+
+    inst.matchDataSender.prevControlWord = ctlWord;
+    inst.matchDataSender.controlWord.Set(ctlWord);
+  }
 }
 
 void JoystickLogSender::Init(wpi::log::DataLog& log, unsigned int stick,
@@ -939,17 +1002,17 @@ void JoystickLogSender::AppendPOVs(const HAL_JoystickPOVs& povs,
 
 void DataLogSender::Init(wpi::log::DataLog& log, bool logJoysticks,
                          int64_t timestamp) {
-  m_logEnabled = wpi::log::BooleanLogEntry{log, "DS:enabled", timestamp};
-  m_logAutonomous = wpi::log::BooleanLogEntry{log, "DS:autonomous", timestamp};
-  m_logTest = wpi::log::BooleanLogEntry{log, "DS:test", timestamp};
-  m_logEstop = wpi::log::BooleanLogEntry{log, "DS:estop", timestamp};
+  m_logControlWord = wpi::log::StructLogEntry<hal::ControlWord>{
+      log, "DS:controlWord", timestamp};
+  m_logOpMode = wpi::log::StringLogEntry{log, "DS:opMode", timestamp};
 
-  // append initial control word values
-  HAL_GetControlWord(&m_prevControlWord);
-  m_logEnabled.Append(m_prevControlWord.enabled, timestamp);
-  m_logAutonomous.Append(m_prevControlWord.autonomous, timestamp);
-  m_logTest.Append(m_prevControlWord.test, timestamp);
-  m_logEstop.Append(m_prevControlWord.eStop, timestamp);
+  // append initial control word value
+  m_prevControlWord = hal::GetControlWord();
+  m_logControlWord.Append(m_prevControlWord);
+
+  // append initial opmode value
+  auto& inst = GetInstance();
+  m_logOpMode.Append(inst.OpModeToString(m_prevControlWord.GetOpModeId()));
 
   m_logJoysticks = logJoysticks;
   if (logJoysticks) {
@@ -968,21 +1031,18 @@ void DataLogSender::Send(uint64_t timestamp) {
   }
 
   // append control word value changes
-  HAL_ControlWord ctlWord;
-  HAL_GetControlWord(&ctlWord);
-  if (ctlWord.enabled != m_prevControlWord.enabled) {
-    m_logEnabled.Append(ctlWord.enabled, timestamp);
+  hal::ControlWord ctlWord = hal::GetControlWord();
+  if (ctlWord != m_prevControlWord) {
+    // append opmode value changes
+    int64_t opModeId = ctlWord.GetOpModeId();
+    if (opModeId != m_prevControlWord.GetOpModeId()) {
+      auto& inst = GetInstance();
+      m_logOpMode.Append(inst.OpModeToString(opModeId));
+    }
+
+    m_prevControlWord = ctlWord;
+    m_logControlWord.Append(ctlWord);
   }
-  if (ctlWord.autonomous != m_prevControlWord.autonomous) {
-    m_logAutonomous.Append(ctlWord.autonomous, timestamp);
-  }
-  if (ctlWord.test != m_prevControlWord.test) {
-    m_logTest.Append(ctlWord.test, timestamp);
-  }
-  if (ctlWord.eStop != m_prevControlWord.eStop) {
-    m_logEstop.Append(ctlWord.eStop, timestamp);
-  }
-  m_prevControlWord = ctlWord;
 
   if (m_logJoysticks) {
     // append joystick value changes

wpilibc/src/main/native/cpp/framework/IterativeRobotBase.cpp

@@ -4,9 +4,9 @@
 
 #include "wpi/framework/IterativeRobotBase.hpp"
 
-#include "wpi/driverstation/DSControlWord.hpp"
 #include "wpi/driverstation/DriverStation.hpp"
 #include "wpi/hal/DriverStation.h"
+#include "wpi/hal/DriverStationTypes.h"
 #include "wpi/nt/NetworkTableInstance.hpp"
 #include "wpi/smartdashboard/SmartDashboard.hpp"
 #include "wpi/system/Errors.hpp"
@@ -98,18 +98,10 @@ void IterativeRobotBase::LoopFunc() {
   DriverStation::RefreshData();
   m_watchdog.Reset();
 
-  // Get current mode
-  DSControlWord word;
-  Mode mode = Mode::kNone;
-  if (word.IsDisabled()) {
-    mode = Mode::kDisabled;
-  } else if (word.IsAutonomous()) {
-    mode = Mode::kAutonomous;
-  } else if (word.IsTeleop()) {
-    mode = Mode::kTeleop;
-  } else if (word.IsTest()) {
-    mode = Mode::kTest;
-  }
+  // Get current mode; treat disabled as unknown
+  wpi::hal::ControlWord word = wpi::hal::GetControlWord();
+  bool enabled = word.IsEnabled();
+  RobotMode mode = enabled ? word.GetRobotMode() : RobotMode::UNKNOWN;
 
   if (!m_calledDsConnected && word.IsDSAttached()) {
     m_calledDsConnected = true;
@@ -117,51 +109,48 @@ void IterativeRobotBase::LoopFunc() {
   }
 
   // If mode changed, call mode exit and entry functions
-  if (m_lastMode != mode) {
+  if (m_lastMode != static_cast<int>(mode)) {
     // Call last mode's exit function
-    if (m_lastMode == Mode::kDisabled) {
+    if (m_lastMode == static_cast<int>(RobotMode::UNKNOWN)) {
       DisabledExit();
-    } else if (m_lastMode == Mode::kAutonomous) {
+    } else if (m_lastMode == static_cast<int>(RobotMode::AUTONOMOUS)) {
       AutonomousExit();
-    } else if (m_lastMode == Mode::kTeleop) {
+    } else if (m_lastMode == static_cast<int>(RobotMode::TELEOPERATED)) {
       TeleopExit();
-    } else if (m_lastMode == Mode::kTest) {
+    } else if (m_lastMode == static_cast<int>(RobotMode::TEST)) {
       TestExit();
     }
 
     // Call current mode's entry function
-    if (mode == Mode::kDisabled) {
+    if (mode == RobotMode::UNKNOWN) {
       DisabledInit();
       m_watchdog.AddEpoch("DisabledInit()");
-    } else if (mode == Mode::kAutonomous) {
+    } else if (mode == RobotMode::AUTONOMOUS) {
       AutonomousInit();
       m_watchdog.AddEpoch("AutonomousInit()");
-    } else if (mode == Mode::kTeleop) {
+    } else if (mode == RobotMode::TELEOPERATED) {
       TeleopInit();
       m_watchdog.AddEpoch("TeleopInit()");
-    } else if (mode == Mode::kTest) {
+    } else if (mode == RobotMode::TEST) {
       TestInit();
       m_watchdog.AddEpoch("TestInit()");
     }
 
-    m_lastMode = mode;
+    m_lastMode = static_cast<int>(mode);
   }
 
   // Call the appropriate function depending upon the current robot mode
-  if (mode == Mode::kDisabled) {
-    HAL_ObserveUserProgramDisabled();
+  HAL_ObserveUserProgram(word.GetValue());
+  if (mode == RobotMode::UNKNOWN) {
     DisabledPeriodic();
     m_watchdog.AddEpoch("DisabledPeriodic()");
-  } else if (mode == Mode::kAutonomous) {
-    HAL_ObserveUserProgramAutonomous();
+  } else if (mode == RobotMode::AUTONOMOUS) {
     AutonomousPeriodic();
     m_watchdog.AddEpoch("AutonomousPeriodic()");
-  } else if (mode == Mode::kTeleop) {
-    HAL_ObserveUserProgramTeleop();
+  } else if (mode == RobotMode::TELEOPERATED) {
     TeleopPeriodic();
     m_watchdog.AddEpoch("TeleopPeriodic()");
-  } else if (mode == Mode::kTest) {
-    HAL_ObserveUserProgramTest();
+  } else if (mode == RobotMode::TEST) {
     TestPeriodic();
     m_watchdog.AddEpoch("TestPeriodic()");
   }

wpilibc/src/main/native/cpp/framework/OpModeRobot.cpp

@@ -0,0 +1,248 @@
+// 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 "wpi/framework/OpModeRobot.hpp"
+
+#include <cstdint>
+#include <cstdlib>
+#include <memory>
+#include <string>
+#include <utility>
+
+#include <fmt/format.h>
+
+#include "wpi/driverstation/DriverStation.hpp"
+#include "wpi/hal/DriverStation.h"
+#include "wpi/hal/DriverStationTypes.h"
+#include "wpi/hal/HALBase.h"
+#include "wpi/hal/Notifier.h"
+#include "wpi/opmode/OpMode.hpp"
+#include "wpi/util/SafeThread.hpp"
+#include "wpi/util/Synchronization.h"
+
+using namespace wpi;
+
+namespace {
+class MonitorThread : public wpi::util::SafeThreadEvent {
+ public:
+  MonitorThread(int64_t modeId, wpi::util::Event& dsEvent,
+                HAL_NotifierHandle notifier, std::weak_ptr<OpMode> activeOpMode)
+      : m_modeId{modeId},
+        m_dsEvent{dsEvent.GetHandle()},
+        m_notifier{notifier},
+        m_activeOpMode{std::move(activeOpMode)} {}
+
+ private:
+  void Main() override {
+    // Wait for DS to disable or change modes
+    WPI_EventHandle events[] = {m_dsEvent, m_stopEvent.GetHandle()};
+    WPI_Handle signaledBuf[2];
+    for (;;) {
+      auto signaled = wpi::util::WaitForObjects(events, signaledBuf);
+      if (signaled.empty()) {
+        return;  // handles destroyed
+      }
+      for (auto signal : signaled) {
+        if ((signal & 0x80000000) != 0 || signal == m_stopEvent.GetHandle()) {
+          return;  // handle destroyed or transitioned
+        }
+      }
+
+      // did the opmode or enable state change?
+      HAL_ControlWord word;
+      HAL_GetUncachedControlWord(&word);
+      if (!HAL_ControlWord_IsEnabled(word) ||
+          HAL_ControlWord_GetOpModeId(word) != m_modeId) {
+        break;
+      }
+    }
+
+    // call opmode stop
+    auto opMode = m_activeOpMode.lock();
+    if (opMode) {
+      opMode->OpModeStop();
+    }
+
+    events[0] = m_notifier;
+    int32_t status = 0;
+    HAL_SetNotifierAlarm(m_notifier, 1000000, 0, false, true, &status);  // 1s
+    auto signaled = wpi::util::WaitForObjects(events, signaledBuf);
+    if (signaled.empty()) {
+      return;  // handles destroyed
+    }
+    for (auto signal : signaled) {
+      if ((signal & 0x80000000) != 0 || signal == m_stopEvent.GetHandle()) {
+        return;  // handle destroyed or transitioned
+      }
+    }
+
+    // if it hasn't transitioned after 1 second, terminate the program
+    WPILIB_ReportError(err::Error, "OpMode did not exit, terminating program");
+    HAL_Shutdown();
+    std::exit(0);
+  }
+
+  int64_t m_modeId;
+  WPI_EventHandle m_dsEvent;
+  HAL_NotifierHandle m_notifier;
+  std::weak_ptr<OpMode> m_activeOpMode;
+};
+}  // namespace
+
+void OpModeRobotBase::StartCompetition() {
+  fmt::print("********** Robot program startup complete **********\n");
+  HAL_ObserveUserProgramStarting();
+
+  wpi::util::Event event;
+  struct DSListener {
+    wpi::util::Event& event;
+    explicit DSListener(wpi::util::Event& event) : event{event} {
+      HAL_ProvideNewDataEventHandle(event.GetHandle());
+    }
+    ~DSListener() { HAL_RemoveNewDataEventHandle(event.GetHandle()); }
+  } listener{event};
+
+  int32_t status = 0;
+  m_notifier = HAL_CreateNotifier(&status);
+  HAL_SetNotifierName(m_notifier, "OpModeRobot", &status);
+
+  int64_t lastModeId = -1;
+  bool calledDriverStationConnected = false;
+  std::shared_ptr<OpMode> opMode;
+  WPI_EventHandle events[] = {event.GetHandle(),
+                              static_cast<WPI_EventHandle>(m_notifier)};
+  WPI_Handle signaledBuf[2];
+  for (;;) {
+    // Wait for new data from the driver station, with 50 ms timeout
+    HAL_SetNotifierAlarm(m_notifier, 50000, 0, false, true, &status);
+    auto signaled = wpi::util::WaitForObjects(events, signaledBuf);
+    if (signaled.empty()) {
+      return;  // handles destroyed
+    }
+    for (auto signal : signaled) {
+      if ((signal & 0x80000000) != 0) {
+        return;  // handle destroyed
+      }
+    }
+
+    // Get the latest control word and opmode
+    DriverStation::RefreshData();
+    hal::ControlWord ctlWord = DriverStation::GetControlWord();
+
+    if (!calledDriverStationConnected && ctlWord.IsDSAttached()) {
+      calledDriverStationConnected = true;
+      DriverStationConnected();
+    }
+
+    int64_t modeId;
+    if (!ctlWord.IsDSAttached()) {
+      modeId = 0;
+    } else {
+      modeId = ctlWord.GetOpModeId();
+    }
+
+    if (!opMode || modeId != lastModeId) {
+      if (opMode) {
+        // no or different opmode selected
+        opMode.reset();
+      }
+
+      if (modeId == 0) {
+        // no opmode selected
+        NonePeriodic();
+        HAL_ObserveUserProgram(ctlWord.GetValue());
+        continue;
+      }
+
+      auto data = m_opModes.lookup(modeId);
+      if (!data.factory) {
+        WPILIB_ReportError(err::Error, "No OpMode found for mode {}", modeId);
+        ctlWord.SetOpModeId(0);
+        HAL_ObserveUserProgram(ctlWord.GetValue());
+        continue;
+      }
+
+      // Instantiate the opmode
+      fmt::print("********** Starting OpMode {} **********\n", data.name);
+      opMode = data.factory();
+      if (!opMode) {
+        // could not construct
+        ctlWord.SetOpModeId(0);
+        HAL_ObserveUserProgram(ctlWord.GetValue());
+        continue;
+      }
+      {
+        std::scoped_lock lock(m_opModeMutex);
+        m_activeOpMode = opMode;
+      }
+      lastModeId = modeId;
+      // Ensure disabledPeriodic is called at least once
+      opMode->DisabledPeriodic();
+    }
+
+    HAL_ObserveUserProgram(ctlWord.GetValue());
+
+    if (ctlWord.IsEnabled()) {
+      // When enabled, call the opmode run function, then close and clear
+      wpi::util::SafeThreadOwner<MonitorThread> monitor;
+      monitor.Start(modeId, event, static_cast<HAL_NotifierHandle>(m_notifier),
+                    opMode);
+      opMode->OpModeRun(modeId);
+      opMode.reset();
+    } else {
+      // When disabled, call the DisabledPeriodic function
+      opMode->DisabledPeriodic();
+    }
+  }
+}
+
+void OpModeRobotBase::EndCompetition() {
+  m_notifier = {};
+  std::shared_ptr<OpMode> opMode;
+  {
+    std::scoped_lock lock(m_opModeMutex);
+    opMode = m_activeOpMode.lock();
+  }
+  if (opMode) {
+    opMode->OpModeStop();
+  }
+}
+
+void OpModeRobotBase::AddOpModeFactory(
+    OpModeFactory factory, RobotMode mode, std::string_view name,
+    std::string_view group, std::string_view description,
+    const wpi::util::Color& textColor,
+    const wpi::util::Color& backgroundColor) {
+  int64_t id = DriverStation::AddOpMode(mode, name, group, description,
+                                        textColor, backgroundColor);
+  if (id != 0) {
+    m_opModes[id] = OpModeData{std::string{name}, std::move(factory)};
+  }
+}
+
+void OpModeRobotBase::AddOpModeFactory(OpModeFactory factory, RobotMode mode,
+                                       std::string_view name,
+                                       std::string_view group,
+                                       std::string_view description) {
+  int64_t id = DriverStation::AddOpMode(mode, name, group, description);
+  if (id != 0) {
+    m_opModes[id] = OpModeData{std::string{name}, std::move(factory)};
+  }
+}
+
+void OpModeRobotBase::RemoveOpMode(RobotMode mode, std::string_view name) {
+  int64_t id = DriverStation::RemoveOpMode(mode, name);
+  if (id != 0) {
+    m_opModes.erase(id);
+  }
+}
+
+void OpModeRobotBase::PublishOpModes() {
+  DriverStation::PublishOpModes();
+}
+
+void OpModeRobotBase::ClearOpModes() {
+  DriverStation::ClearOpModes();
+  m_opModes.clear();
+}

wpilibc/src/main/native/cpp/framework/RobotState.cpp

@@ -1,33 +0,0 @@
-// 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 "wpi/framework/RobotState.hpp"
-
-#include "wpi/driverstation/DriverStation.hpp"
-
-using namespace wpi;
-
-bool RobotState::IsDisabled() {
-  return DriverStation::IsDisabled();
-}
-
-bool RobotState::IsEnabled() {
-  return DriverStation::IsEnabled();
-}
-
-bool RobotState::IsEStopped() {
-  return DriverStation::IsEStopped();
-}
-
-bool RobotState::IsTeleop() {
-  return DriverStation::IsTeleop();
-}
-
-bool RobotState::IsAutonomous() {
-  return DriverStation::IsAutonomous();
-}
-
-bool RobotState::IsTest() {
-  return DriverStation::IsTest();
-}

wpilibc/src/main/native/cpp/hardware/motor/MotorSafety.cpp

@@ -4,11 +4,11 @@
 
 #include "wpi/hardware/motor/MotorSafety.hpp"
 
-#include <algorithm>
 #include <utility>
 
 #include "wpi/driverstation/DriverStation.hpp"
 #include "wpi/hal/DriverStation.h"
+#include "wpi/hal/DriverStationTypes.h"
 #include "wpi/system/Errors.hpp"
 #include "wpi/util/SafeThread.hpp"
 #include "wpi/util/SmallPtrSet.hpp"
@@ -32,9 +32,9 @@ void Thread::Main() {
     bool signaled = wpi::util::WaitForObject(event.GetHandle(), 0.1, &timedOut);
     if (signaled) {
       HAL_ControlWord controlWord;
-      std::memset(&controlWord, 0, sizeof(controlWord));
       HAL_GetControlWord(&controlWord);
-      if (!(controlWord.enabled && controlWord.dsAttached)) {
+      if (!HAL_ControlWord_IsEnabled(controlWord) ||
+          !HAL_ControlWord_IsDSAttached(controlWord)) {
         safetyCounter = 0;
       }
       if (++safetyCounter >= 4) {

wpilibc/src/main/native/cpp/internal/DriverStationModeThread.cpp

@@ -10,55 +10,30 @@
 
 using namespace wpi::internal;
 
-DriverStationModeThread::DriverStationModeThread() {
+DriverStationModeThread::DriverStationModeThread(wpi::hal::ControlWord word)
+    : m_userControlWord{word.GetValue().value} {
+  HAL_ProvideNewDataEventHandle(m_event.GetHandle());
   m_keepAlive = true;
-  m_thread = std::thread{[&] { Run(); }};
+  m_thread = std::thread{[this] { Run(); }};
 }
 
 DriverStationModeThread::~DriverStationModeThread() {
+  HAL_RemoveNewDataEventHandle(m_event.GetHandle());
   m_keepAlive = false;
+  m_event.Set();
   if (m_thread.joinable()) {
     m_thread.join();
   }
 }
 
-void DriverStationModeThread::InDisabled(bool entering) {
-  m_userInDisabled = entering;
-}
-
-void DriverStationModeThread::InAutonomous(bool entering) {
-  m_userInAutonomous = entering;
-}
-
-void DriverStationModeThread::InTeleop(bool entering) {
-  m_userInTeleop = entering;
-}
-
-void DriverStationModeThread::InTest(bool entering) {
-  m_userInTest = entering;
-}
-
 void DriverStationModeThread::Run() {
-  wpi::util::Event event{false, false};
-  HAL_ProvideNewDataEventHandle(event.GetHandle());
-
-  while (m_keepAlive.load()) {
+  for (;;) {
     bool timedOut = false;
-    wpi::util::WaitForObject(event.GetHandle(), 0.1, &timedOut);
+    wpi::util::WaitForObject(m_event.GetHandle(), 0.1, &timedOut);
+    if (!m_keepAlive) {
+      break;
+    }
     wpi::DriverStation::RefreshData();
-    if (m_userInDisabled) {
-      HAL_ObserveUserProgramDisabled();
-    }
-    if (m_userInAutonomous) {
-      HAL_ObserveUserProgramAutonomous();
-    }
-    if (m_userInTeleop) {
-      HAL_ObserveUserProgramTeleop();
-    }
-    if (m_userInTest) {
-      HAL_ObserveUserProgramTest();
-    }
+    HAL_ObserveUserProgram({.value = m_userControlWord});
   }
-
-  HAL_RemoveNewDataEventHandle(event.GetHandle());
 }

wpilibc/src/main/native/cpp/opmode/LinearOpMode.cpp

@@ -0,0 +1,21 @@
+// 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 "wpi/opmode/LinearOpMode.hpp"
+
+#include "wpi/hal/DriverStation.h"
+#include "wpi/internal/DriverStationModeThread.hpp"
+
+using namespace wpi;
+
+void LinearOpMode::OpModeRun(int64_t opModeId) {
+  auto word = wpi::hal::GetControlWord();
+  word.SetOpModeId(opModeId);
+  internal::DriverStationModeThread bgThread{word};
+  Run();
+}
+
+void LinearOpMode::OpModeStop() {
+  m_running = false;
+}

wpilibc/src/main/native/cpp/opmode/PeriodicOpMode.cpp

@@ -0,0 +1,158 @@
+// 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 "wpi/opmode/PeriodicOpMode.hpp"
+
+#include <utility>
+
+#include "wpi/driverstation/DriverStation.hpp"
+#include "wpi/hal/DriverStation.h"
+#include "wpi/hal/UsageReporting.h"
+#include "wpi/nt/NetworkTableInstance.hpp"
+#include "wpi/smartdashboard/SmartDashboard.hpp"
+#include "wpi/system/Errors.hpp"
+#include "wpi/system/RobotController.hpp"
+
+using namespace wpi;
+
+PeriodicOpMode::Callback::Callback(std::function<void()> func,
+                                   std::chrono::microseconds startTime,
+                                   std::chrono::microseconds period,
+                                   std::chrono::microseconds offset)
+    : func{std::move(func)},
+      period{period},
+      expirationTime(
+          startTime + offset + period +
+          (std::chrono::microseconds{wpi::RobotController::GetFPGATime()} -
+           startTime) /
+              period * period) {}
+
+PeriodicOpMode::~PeriodicOpMode() {
+  if (m_notifier != HAL_kInvalidHandle) {
+    HAL_DestroyNotifier(m_notifier);
+  }
+}
+
+PeriodicOpMode::PeriodicOpMode(wpi::units::second_t period)
+    : m_period{period},
+      m_watchdog(period, [this] { PrintLoopOverrunMessage(); }) {
+  m_startTime = std::chrono::microseconds{RobotController::GetFPGATime()};
+  AddPeriodic([=, this] { LoopFunc(); }, period);
+
+  int32_t status = 0;
+  m_notifier = HAL_CreateNotifier(&status);
+  WPILIB_CheckErrorStatus(status, "CreateNotifier");
+  HAL_SetNotifierName(m_notifier, "PeriodicOpMode", &status);
+
+  HAL_ReportUsage("OpMode", "PeriodicOpMode");
+}
+
+void PeriodicOpMode::AddPeriodic(std::function<void()> callback,
+                                 wpi::units::second_t period,
+                                 wpi::units::second_t offset) {
+  m_callbacks.emplace(
+      callback, m_startTime,
+      std::chrono::microseconds{static_cast<int64_t>(period.value() * 1e6)},
+      std::chrono::microseconds{static_cast<int64_t>(offset.value() * 1e6)});
+}
+
+void PeriodicOpMode::LoopFunc() {
+  DriverStation::RefreshData();
+  HAL_ControlWord word;
+  HAL_GetControlWord(&word);
+  HAL_ControlWord_SetOpModeId(&word, m_opModeId);
+  HAL_ObserveUserProgram(word);
+
+  if (!DriverStation::IsEnabled() ||
+      DriverStation::GetOpModeId() != m_opModeId) {
+    m_running = false;
+    return;
+  }
+
+  m_watchdog.Reset();
+  Periodic();
+  m_watchdog.AddEpoch("Periodic()");
+
+  SmartDashboard::UpdateValues();
+  m_watchdog.AddEpoch("SmartDashboard::UpdateValues()");
+
+  // if constexpr (IsSimulation()) {
+  //   HAL_SimPeriodicBefore();
+  //   SimulationPeriodic();
+  //   HAL_SimPeriodicAfter();
+  //   m_watchdog.AddEpoch("SimulationPeriodic()");
+  // }
+
+  m_watchdog.Disable();
+
+  // Flush NetworkTables
+  nt::NetworkTableInstance::GetDefault().FlushLocal();
+
+  // Warn on loop time overruns
+  if (m_watchdog.IsExpired()) {
+    m_watchdog.PrintEpochs();
+  }
+}
+
+void PeriodicOpMode::OpModeRun(int64_t opModeId) {
+  m_opModeId = opModeId;
+
+  Start();
+
+  while (m_running) {
+    // We don't have to check there's an element in the queue first because
+    // there's always at least one (the constructor adds one). It's reenqueued
+    // at the end of the loop.
+    auto callback = m_callbacks.pop();
+
+    int32_t status = 0;
+    HAL_SetNotifierAlarm(m_notifier, callback.expirationTime.count(), 0, true,
+                         true, &status);
+    WPILIB_CheckErrorStatus(status, "SetNotifierAlarm");
+
+    if (WPI_WaitForObject(m_notifier) == 0) {
+      break;
+    }
+
+    m_loopStartTimeUs = RobotController::GetFPGATime();
+    std::chrono::microseconds currentTime{m_loopStartTimeUs};
+
+    callback.func();
+
+    // Increment the expiration time by the number of full periods it's behind
+    // plus one to avoid rapid repeat fires from a large loop overrun. We assume
+    // currentTime ≥ expirationTime rather than checking for it since the
+    // callback wouldn't be running otherwise.
+    callback.expirationTime +=
+        callback.period + (currentTime - callback.expirationTime) /
+                              callback.period * callback.period;
+    m_callbacks.push(std::move(callback));
+
+    // Process all other callbacks that are ready to run
+    while (m_callbacks.top().expirationTime <= currentTime) {
+      callback = m_callbacks.pop();
+
+      callback.func();
+
+      callback.expirationTime +=
+          callback.period + (currentTime - callback.expirationTime) /
+                                callback.period * callback.period;
+      m_callbacks.push(std::move(callback));
+    }
+  }
+  End();
+}
+
+void PeriodicOpMode::OpModeStop() {
+  HAL_DestroyNotifier(m_notifier);
+  m_notifier = HAL_kInvalidHandle;
+}
+
+void PeriodicOpMode::PrintLoopOverrunMessage() {
+  WPILIB_ReportWarning("Loop time of {:.6f}s overrun", m_period.value());
+}
+
+void PeriodicOpMode::PrintWatchdogEpochs() {
+  m_watchdog.PrintEpochs();
+}

wpilibc/src/main/native/cpp/simulation/CallbackStore.cpp

@@ -21,6 +21,13 @@ void wpi::sim::ConstBufferCallbackStoreThunk(const char* name, void* param,
                                                                count);
 }
 
+void wpi::sim::OpModeOptionsCallbackStoreThunk(const char* name, void* param,
+                                               const HAL_OpModeOption* opmodes,
+                                               int32_t count) {
+  reinterpret_cast<CallbackStore*>(param)->opModeOptionsCallback(
+      name, {opmodes, opmodes + count});
+}
+
 CallbackStore::CallbackStore(int32_t i, NotifyCallback cb,
                              CancelCallbackNoIndexFunc ccf)
     : index(i), callback(std::move(cb)), cancelType(NoIndex) {
@@ -66,6 +73,12 @@ CallbackStore::CallbackStore(int32_t i, int32_t c, int32_t u,
   this->cccf = ccf;
 }
 
+CallbackStore::CallbackStore(int32_t u, OpModeOptionsCallback cb,
+                             CancelCallbackNoIndexFunc ccf)
+    : uid{u}, opModeOptionsCallback{std::move(cb)}, cancelType{NoIndex} {
+  this->ccnif = ccf;
+}
+
 CallbackStore::~CallbackStore() {
   switch (cancelType) {
     case Normal:

wpilibc/src/main/native/cpp/simulation/DriverStationSim.cpp

@@ -31,38 +31,21 @@ void DriverStationSim::SetEnabled(bool enabled) {
   HALSIM_SetDriverStationEnabled(enabled);
 }
 
-std::unique_ptr<CallbackStore> DriverStationSim::RegisterAutonomousCallback(
+std::unique_ptr<CallbackStore> DriverStationSim::RegisterRobotModeCallback(
     NotifyCallback callback, bool initialNotify) {
   auto store = std::make_unique<CallbackStore>(
-      -1, callback, &HALSIM_CancelDriverStationAutonomousCallback);
-  store->SetUid(HALSIM_RegisterDriverStationAutonomousCallback(
+      -1, callback, &HALSIM_CancelDriverStationRobotModeCallback);
+  store->SetUid(HALSIM_RegisterDriverStationRobotModeCallback(
       &CallbackStoreThunk, store.get(), initialNotify));
   return store;
 }
 
-bool DriverStationSim::GetAutonomous() {
-  return HALSIM_GetDriverStationAutonomous();
+HAL_RobotMode DriverStationSim::GetRobotMode() {
+  return HALSIM_GetDriverStationRobotMode();
 }
 
-void DriverStationSim::SetAutonomous(bool autonomous) {
-  HALSIM_SetDriverStationAutonomous(autonomous);
-}
-
-std::unique_ptr<CallbackStore> DriverStationSim::RegisterTestCallback(
-    NotifyCallback callback, bool initialNotify) {
-  auto store = std::make_unique<CallbackStore>(
-      -1, callback, &HALSIM_CancelDriverStationTestCallback);
-  store->SetUid(HALSIM_RegisterDriverStationTestCallback(
-      &CallbackStoreThunk, store.get(), initialNotify));
-  return store;
-}
-
-bool DriverStationSim::GetTest() {
-  return HALSIM_GetDriverStationTest();
-}
-
-void DriverStationSim::SetTest(bool test) {
-  HALSIM_SetDriverStationTest(test);
+void DriverStationSim::SetRobotMode(HAL_RobotMode robotMode) {
+  HALSIM_SetDriverStationRobotMode(robotMode);
 }
 
 std::unique_ptr<CallbackStore> DriverStationSim::RegisterEStopCallback(
@@ -152,6 +135,38 @@ void DriverStationSim::SetMatchTime(double matchTime) {
   HALSIM_SetDriverStationMatchTime(matchTime);
 }
 
+std::unique_ptr<CallbackStore> DriverStationSim::RegisterOpModeCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      -1, callback, &HALSIM_CancelDriverStationOpModeCallback);
+  store->SetUid(HALSIM_RegisterDriverStationOpModeCallback(
+      &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+int64_t DriverStationSim::GetOpMode() {
+  return HALSIM_GetDriverStationOpMode();
+}
+
+void DriverStationSim::SetOpMode(int64_t opmode) {
+  HALSIM_SetDriverStationOpMode(opmode);
+}
+
+std::unique_ptr<CallbackStore> DriverStationSim::RegisterOpModeOptionsCallback(
+    OpModeOptionsCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      -1, callback, &HALSIM_CancelOpModeOptionsCallback);
+  store->SetUid(HALSIM_RegisterOpModeOptionsCallback(
+      &OpModeOptionsCallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+OpModeOptions DriverStationSim::GetOpModeOptions() {
+  int32_t len;
+  auto options = HALSIM_GetOpModeOptions(&len);
+  return {options, len};
+}
+
 void DriverStationSim::NotifyNewData() {
   wpi::util::Event waitEvent{true};
   HAL_ProvideNewDataEventHandle(waitEvent.GetHandle());

wpilibc/src/main/native/cpp/simulation/SimHooks.cpp

@@ -24,6 +24,14 @@ bool GetProgramStarted() {
   return HALSIM_GetProgramStarted();
 }
 
+void SetProgramState(wpi::hal::ControlWord controlWord) {
+  wpi::hal::sim::SetProgramState(controlWord);
+}
+
+wpi::hal::ControlWord GetProgramState() {
+  return wpi::hal::sim::GetProgramState();
+}
+
 void RestartTiming() {
   HALSIM_RestartTiming();
 }