[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();
 }

wpilibc/src/main/native/cppcs/RobotBase.cpp

@@ -4,6 +4,8 @@
 
 #include "wpi/framework/RobotBase.hpp"
 
+#include <stdint.h>
+
 #ifdef __FRC_SYSTEMCORE__
 #include <dlfcn.h>
 #endif
@@ -140,38 +142,46 @@ static void SetupMathShared() {
       std::make_unique<WPILibMathShared>());
 }
 
-bool RobotBase::IsEnabled() const {
+bool RobotBase::IsEnabled() {
   return DriverStation::IsEnabled();
 }
 
-bool RobotBase::IsDisabled() const {
+bool RobotBase::IsDisabled() {
   return DriverStation::IsDisabled();
 }
 
-bool RobotBase::IsAutonomous() const {
+bool RobotBase::IsAutonomous() {
   return DriverStation::IsAutonomous();
 }
 
-bool RobotBase::IsAutonomousEnabled() const {
+bool RobotBase::IsAutonomousEnabled() {
   return DriverStation::IsAutonomousEnabled();
 }
 
-bool RobotBase::IsTeleop() const {
+bool RobotBase::IsTeleop() {
   return DriverStation::IsTeleop();
 }
 
-bool RobotBase::IsTeleopEnabled() const {
+bool RobotBase::IsTeleopEnabled() {
   return DriverStation::IsTeleopEnabled();
 }
 
-bool RobotBase::IsTest() const {
+bool RobotBase::IsTest() {
   return DriverStation::IsTest();
 }
 
-bool RobotBase::IsTestEnabled() const {
+bool RobotBase::IsTestEnabled() {
   return DriverStation::IsTestEnabled();
 }
 
+int64_t RobotBase::GetOpModeId() {
+  return DriverStation::GetOpModeId();
+}
+
+std::string RobotBase::GetOpMode() {
+  return DriverStation::GetOpMode();
+}
+
 std::thread::id RobotBase::GetThreadId() {
   return m_threadId;
 }

wpilibc/src/main/native/include/wpi/driverstation/DSControlWord.hpp

@@ -1,102 +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.
-
-#pragma once
-
-#include "wpi/hal/DriverStationTypes.h"
-
-namespace wpi {
-
-/**
- * A wrapper around Driver Station control word.
- */
-class DSControlWord {
- public:
-  /**
-   * DSControlWord constructor.
-   *
-   * Upon construction, the current Driver Station control word is read and
-   * stored internally.
-   */
-  DSControlWord();
-
-  /**
-   * Check if the DS has enabled the robot.
-   *
-   * @return True if the robot is enabled and the DS is connected
-   */
-  bool IsEnabled() const;
-
-  /**
-   * Check if the robot is disabled.
-   *
-   * @return True if the robot is explicitly disabled or the DS is not connected
-   */
-  bool IsDisabled() const;
-
-  /**
-   * Check if the robot is e-stopped.
-   *
-   * @return True if the robot is e-stopped
-   */
-  bool IsEStopped() const;
-
-  /**
-   * Check if the DS is commanding autonomous mode.
-   *
-   * @return True if the robot is being commanded to be in autonomous mode
-   */
-  bool IsAutonomous() const;
-
-  /**
-   * Check if the DS is commanding autonomous mode and if it has enabled the
-   * robot.
-   *
-   * @return True if the robot is being commanded to be in autonomous mode and
-   * enabled.
-   */
-  bool IsAutonomousEnabled() const;
-
-  /**
-   * Check if the DS is commanding teleop mode.
-   *
-   * @return True if the robot is being commanded to be in teleop mode
-   */
-  bool IsTeleop() const;
-
-  /**
-   * Check if the DS is commanding teleop mode and if it has enabled the robot.
-   *
-   * @return True if the robot is being commanded to be in teleop mode and
-   * enabled.
-   */
-  bool IsTeleopEnabled() const;
-
-  /**
-   * Check if the DS is commanding test mode.
-   *
-   * @return True if the robot is being commanded to be in test mode
-   */
-  bool IsTest() const;
-
-  /**
-   * Check if the DS is attached.
-   *
-   * @return True if the DS is connected to the robot
-   */
-  bool IsDSAttached() const;
-
-  /**
-   * Is the driver station attached to a Field Management System?
-   *
-   * @return True if the robot is competing on a field being controlled by a
-   *         Field Management System
-   */
-  bool IsFMSAttached() const;
-
- private:
-  HAL_ControlWord m_controlWord;
-};
-
-}  // namespace wpi

wpilibc/src/main/native/include/wpi/driverstation/DriverStation.hpp

@@ -6,7 +6,9 @@
 
 #include <optional>
 #include <string>
+#include <string_view>
 
+#include "wpi/hal/DriverStation.h"
 #include "wpi/hal/DriverStationTypes.h"
 #include "wpi/math/geometry/Rotation2d.hpp"
 #include "wpi/units/time.hpp"
@@ -16,8 +18,14 @@ namespace wpi::log {
 class DataLog;
 }  // namespace wpi::log
 
+namespace wpi::util {
+class Color;
+}  // namespace wpi::util
+
 namespace wpi {
 
+using wpi::hal::RobotMode;
+
 /**
  * Provide access to the network communication data to / from the Driver
  * Station.
@@ -313,28 +321,41 @@ class DriverStation final {
    *
    * @return True if the robot is enabled and the DS is connected
    */
-  static bool IsEnabled();
+  static bool IsEnabled() {
+    hal::ControlWord controlWord = GetControlWord();
+    return controlWord.IsEnabled() && controlWord.IsDSAttached();
+  }
 
   /**
    * Check if the robot is disabled.
    *
    * @return True if the robot is explicitly disabled or the DS is not connected
    */
-  static bool IsDisabled();
+  static bool IsDisabled() { return !IsEnabled(); }
 
   /**
    * Check if the robot is e-stopped.
    *
    * @return True if the robot is e-stopped
    */
-  static bool IsEStopped();
+  static bool IsEStopped() { return GetControlWord().IsEStopped(); }
+
+  /**
+   * Gets the current robot mode.
+   *
+   * <p>Note that this does not indicate whether the robot is enabled or
+   * disabled.
+   *
+   * @return robot mode
+   */
+  static RobotMode GetRobotMode() { return GetControlWord().GetRobotMode(); }
 
   /**
    * Check if the DS is commanding autonomous mode.
    *
    * @return True if the robot is being commanded to be in autonomous mode
    */
-  static bool IsAutonomous();
+  static bool IsAutonomous() { return GetControlWord().IsAutonomous(); }
 
   /**
    * Check if the DS is commanding autonomous mode and if it has enabled the
@@ -343,14 +364,16 @@ class DriverStation final {
    * @return True if the robot is being commanded to be in autonomous mode and
    * enabled.
    */
-  static bool IsAutonomousEnabled();
+  static bool IsAutonomousEnabled() {
+    return GetControlWord().IsAutonomousEnabled();
+  }
 
   /**
    * Check if the DS is commanding teleop mode.
    *
    * @return True if the robot is being commanded to be in teleop mode
    */
-  static bool IsTeleop();
+  static bool IsTeleop() { return GetControlWord().IsTeleop(); }
 
   /**
    * Check if the DS is commanding teleop mode and if it has enabled the robot.
@@ -358,14 +381,14 @@ class DriverStation final {
    * @return True if the robot is being commanded to be in teleop mode and
    * enabled.
    */
-  static bool IsTeleopEnabled();
+  static bool IsTeleopEnabled() { return GetControlWord().IsTeleopEnabled(); }
 
   /**
    * Check if the DS is commanding test mode.
    *
    * @return True if the robot is being commanded to be in test mode
    */
-  static bool IsTest();
+  static bool IsTest() { return GetControlWord().IsTest(); }
 
   /**
    * Check if the DS is commanding Test mode and if it has enabled the robot.
@@ -373,14 +396,112 @@ class DriverStation final {
    * @return True if the robot is being commanded to be in Test mode and
    * enabled.
    */
-  static bool IsTestEnabled();
+  static bool IsTestEnabled() { return GetControlWord().IsTestEnabled(); }
+
+  /**
+   * Adds an operating mode option. It's necessary to call PublishOpModes() to
+   * make the added modes visible to the driver station.
+   *
+   * @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
+   * @return unique ID used to later identify the operating mode; if a blank
+   * name is passed, 0 is returned; identical names for the same robot
+   * mode result in a 0 return value
+   */
+  static int64_t 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);
+
+  /**
+   * Adds an operating mode option. It's necessary to call PublishOpModes() to
+   * make the added modes visible to the driver station.
+   *
+   * @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
+   * @return unique ID used to later identify the operating mode; if a blank
+   * name is passed, 0 is returned; identical names for the same robot
+   * mode result in a 0 return value
+   */
+  static int64_t AddOpMode(RobotMode mode, std::string_view name,
+                           std::string_view group = {},
+                           std::string_view description = {});
+
+  /**
+   * Removes an operating mode option. It's necessary to call PublishOpModes()
+   * to make the removed mode no longer visible to the driver station.
+   *
+   * @param mode robot mode
+   * @param name name of the operating mode
+   * @return unique ID for the opmode, or 0 if not found
+   */
+  static int64_t RemoveOpMode(RobotMode mode, std::string_view name);
+
+  /**
+   * Publishes the operating mode options to the driver station.
+   */
+  static void PublishOpModes();
+
+  /**
+   * Clears all operating mode options and publishes an empty list to the driver
+   * station.
+   */
+  static void ClearOpModes();
+
+  /**
+   * Gets the operating mode selected on the driver station. Note this does not
+   * mean the robot is enabled; use IsEnabled() for that. In a match, this will
+   * indicate the operating mode selected for auto before the match starts
+   * (i.e., while the robot is disabled in auto mode); after the auto period
+   * ends, this will change to reflect the operating mode selected for teleop.
+   *
+   * @return the unique ID provided by the AddOpMode() function; may return 0 or
+   * a unique ID not added, so callers should be prepared to handle that case
+   */
+  static int64_t GetOpModeId() { return GetControlWord().GetOpModeId(); }
+
+  /**
+   * Gets the operating mode selected on the driver station. Note this does not
+   * mean the robot is enabled; use IsEnabled() for that. In a match, this will
+   * indicate the operating mode selected for auto before the match starts
+   * (i.e., while the robot is disabled in auto mode); after the auto period
+   * ends, this will change to reflect the operating mode selected for teleop.
+   *
+   * @return Operating mode string; may return a string not in the list of
+   * options, so callers should be prepared to handle that case
+   */
+  static std::string GetOpMode();
+
+  /**
+   * Check to see if the selected operating mode is a particular value. Note
+   * this does not mean the robot is enabled; use IsEnabled() for that.
+   *
+   * @param id operating mode unique ID
+   * @return True if that mode is the current mode
+   */
+  static bool IsOpMode(int64_t id) { return GetOpModeId() == id; }
+
+  /**
+   * Check to see if the selected operating mode is a particular value. Note
+   * this does not mean the robot is enabled; use IsEnabled() for that.
+   *
+   * @param mode operating mode
+   * @return True if that mode is the current mode
+   */
+  static bool IsOpMode(std::string_view mode) { return GetOpMode() == mode; }
 
   /**
    * Check if the DS is attached.
    *
    * @return True if the DS is connected to the robot
    */
-  static bool IsDSAttached();
+  static bool IsDSAttached() { return GetControlWord().IsDSAttached(); }
 
   /**
    * Is the driver station attached to a Field Management System?
@@ -388,7 +509,7 @@ class DriverStation final {
    * @return True if the robot is competing on a field being controlled by a
    *         Field Management System
    */
-  static bool IsFMSAttached();
+  static bool IsFMSAttached() { return GetControlWord().IsFMSAttached(); }
 
   /**
    * Returns the game specific message provided by the FMS.
@@ -482,6 +603,13 @@ class DriverStation final {
    */
   static double GetBatteryVoltage();
 
+  /**
+   * Get the current control word.
+   *
+   * @return control word
+   */
+  static hal::ControlWord GetControlWord() { return hal::GetControlWord(); }
+
   /**
    * Copy data from the DS task for the user. If no new data exists, it will
    * just be returned, otherwise the data will be copied from the DS polling

wpilibc/src/main/native/include/wpi/framework/IterativeRobotBase.hpp

@@ -226,9 +226,7 @@ class IterativeRobotBase : public RobotBase {
   void LoopFunc();
 
  private:
-  enum class Mode { kNone, kDisabled, kAutonomous, kTeleop, kTest };
-
-  Mode m_lastMode = Mode::kNone;
+  int m_lastMode = -1;
   wpi::units::second_t m_period;
   Watchdog m_watchdog;
   bool m_ntFlushEnabled = true;

wpilibc/src/main/native/include/wpi/framework/OpModeRobot.hpp

@@ -0,0 +1,234 @@
+// 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 <concepts>
+#include <cstdint>
+#include <functional>
+#include <memory>
+#include <string>
+
+#include "wpi/framework/RobotBase.hpp"
+#include "wpi/hal/DriverStationTypes.h"
+#include "wpi/hal/Notifier.h"
+#include "wpi/opmode/OpMode.hpp"
+#include "wpi/util/DenseMap.hpp"
+#include "wpi/util/mutex.hpp"
+
+namespace wpi::util {
+class Color;
+}  // namespace wpi::util
+
+namespace wpi {
+
+using RobotMode = wpi::hal::RobotMode;
+
+namespace detail {
+template <typename T>
+concept OpModeDerived = std::derived_from<T, OpMode>;
+template <typename T>
+concept NoArgOpMode = std::constructible_from<T> && OpModeDerived<T>;
+template <typename T, typename R>
+concept OneArgOpMode = std::constructible_from<T, R&> && OpModeDerived<T>;
+}  // namespace detail
+
+/**
+ * Concept indicating a class is derived from OpMode and has either a
+ * no-argument constructor or a constructorthat accepts R&.
+ *
+ * @tparam T opmode class
+ * @tparam R robot class
+ */
+template <typename T, typename R>
+concept ConstructibleOpMode =
+    detail::NoArgOpMode<T> || detail::OneArgOpMode<T, R>;
+
+/**
+ * OpModeRobotBase is the non-templated base class for OpModeRobot. Users should
+ * generally prefer using OpModeRobot instead of this class.
+ *
+ * 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.
+ */
+class OpModeRobotBase : public RobotBase {
+ public:
+  using OpModeFactory = std::function<std::unique_ptr<OpMode>()>;
+
+  /**
+   * Provide an alternate "main loop" via StartCompetition().
+   */
+  void StartCompetition() override;
+
+  /**
+   * Ends the main loop in StartCompetition().
+   */
+  void EndCompetition() override;
+
+  /**
+   * Constructor.
+   */
+  OpModeRobotBase() = default;
+  OpModeRobotBase(OpModeRobotBase&&) = delete;
+  OpModeRobotBase& operator=(OpModeRobotBase&&) = delete;
+
+  /**
+   * Function called exactly once after the DS is connected.
+   *
+   * Code that needs to know the DS state should go here.
+   *
+   * Users should override this method for initialization that needs to occur
+   * after the DS is connected, such as needing the alliance information.
+   */
+  virtual void DriverStationConnected() {}
+
+  /**
+   * Function called periodically anytime when no opmode is selected, including
+   * when the Driver Station is disconnected.
+   */
+  virtual void NonePeriodic() {}
+
+  /**
+   * Adds an operating mode option using a factory function that creates the
+   * opmode. It's necessary to call PublishOpModes() to make the added modes
+   * visible to the driver station.
+   *
+   * @param factory factory function
+   * @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
+   */
+  void 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);
+
+  /**
+   * Adds an operating mode option using a factory function that creates the
+   * opmode. It's necessary to call PublishOpModes() to make the added modes
+   * visible to the driver station.
+   *
+   * @param factory factory function
+   * @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
+   */
+  void AddOpModeFactory(OpModeFactory factory, RobotMode mode,
+                        std::string_view name, std::string_view group = {},
+                        std::string_view description = {});
+
+  /**
+   * Removes an operating mode option. It's necessary to call PublishOpModes()
+   * to make the removed mode no longer visible to the driver station.
+   *
+   * @param mode robot mode
+   * @param name name of the operating mode
+   */
+  void RemoveOpMode(RobotMode mode, std::string_view name);
+
+  /**
+   * Publishes the operating mode options to the driver station.
+   */
+  void PublishOpModes();
+
+  /**
+   * Clears all operating mode options and publishes an empty list to the driver
+   * station.
+   */
+  void ClearOpModes();
+
+ private:
+  struct OpModeData {
+    std::string name;
+    OpModeFactory factory;
+  };
+  wpi::util::DenseMap<int64_t, OpModeData> m_opModes;
+  wpi::hal::Handle<HAL_NotifierHandle, HAL_DestroyNotifier> m_notifier;
+  wpi::util::mutex m_opModeMutex;
+  std::weak_ptr<OpMode> m_activeOpMode;
+};
+
+/**
+ * OpModeRobot implements the opmode-based robot program framework.
+ *
+ * The OpModeRobot class is intended to be subclassed by a user creating a robot
+ * program. Users must provide their derived class as a template parameter to
+ * this class.
+ *
+ * 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.
+ *
+ * @tparam Derived derived class
+ */
+template <typename Derived>
+class OpModeRobot : public OpModeRobotBase {
+ public:
+  /**
+   * Adds an operating mode option. It's necessary to call PublishOpModes() to
+   * make the added modes visible to the driver station.
+   *
+   * @tparam T opmode class; must be a public, non-abstract subclass of OpMode
+   * with a public 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
+   */
+  template <ConstructibleOpMode<Derived> T>
+  void 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) {
+    if constexpr (detail::OneArgOpMode<T, Derived>) {
+      AddOpModeFactory(
+          [this] { return std::make_unique<T>(*static_cast<Derived*>(this)); },
+          mode, name, group, description, textColor, backgroundColor);
+    } else if constexpr (detail::NoArgOpMode<T>) {
+      AddOpModeFactory([] { return std::make_unique<T>(); }, mode, name, group,
+                       description, textColor, backgroundColor);
+    }
+  }
+
+  /**
+   * Adds an operating mode option. It's necessary to call PublishOpModes() to
+   * make the added modes visible to the driver station.
+   *
+   * @tparam T opmode class; must be a public, non-abstract subclass of OpMode
+   * with a public 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
+   */
+  template <ConstructibleOpMode<Derived> T>
+  void AddOpMode(RobotMode mode, std::string_view name,
+                 std::string_view group = {},
+                 std::string_view description = {}) {
+    if constexpr (detail::OneArgOpMode<T, Derived>) {
+      AddOpModeFactory(
+          [this] { return std::make_unique<T>(*static_cast<Derived*>(this)); },
+          mode, name, group, description);
+    } else if constexpr (detail::NoArgOpMode<T>) {
+      AddOpModeFactory([] { return std::make_unique<T>(); }, mode, name, group,
+                       description);
+    }
+  }
+};
+
+}  // namespace wpi

wpilibc/src/main/native/include/wpi/framework/RobotBase.hpp

@@ -5,6 +5,7 @@
 #pragma once
 
 #include <chrono>
+#include <string>
 #include <thread>
 
 #include "wpi/hal/DriverStation.h"
@@ -148,14 +149,14 @@ class RobotBase {
    *
    * @return True if the Robot is currently enabled by the Driver Station.
    */
-  bool IsEnabled() const;
+  static bool IsEnabled();
 
   /**
    * Determine if the Robot is currently disabled.
    *
    * @return True if the Robot is currently disabled by the Driver Station.
    */
-  bool IsDisabled() const;
+  static bool IsDisabled();
 
   /**
    * Determine if the robot is currently in Autonomous mode.
@@ -163,7 +164,7 @@ class RobotBase {
    * @return True if the robot is currently operating Autonomously as determined
    *         by the Driver Station.
    */
-  bool IsAutonomous() const;
+  static bool IsAutonomous();
 
   /**
    * Determine if the robot is currently in Autonomous mode and enabled.
@@ -171,7 +172,7 @@ class RobotBase {
    * @return True if the robot us currently operating Autonomously while enabled
    * as determined by the Driver Station.
    */
-  bool IsAutonomousEnabled() const;
+  static bool IsAutonomousEnabled();
 
   /**
    * Determine if the robot is currently in Operator Control mode.
@@ -179,7 +180,7 @@ class RobotBase {
    * @return True if the robot is currently operating in Tele-Op mode as
    *         determined by the Driver Station.
    */
-  bool IsTeleop() const;
+  static bool IsTeleop();
 
   /**
    * Determine if the robot is current in Operator Control mode and enabled.
@@ -187,7 +188,7 @@ class RobotBase {
    * @return True if the robot is currently operating in Tele-Op mode while
    * enabled as determined by the Driver Station.
    */
-  bool IsTeleopEnabled() const;
+  static bool IsTeleopEnabled();
 
   /**
    * Determine if the robot is currently in Test mode.
@@ -195,7 +196,7 @@ class RobotBase {
    * @return True if the robot is currently running in Test mode as determined
    * by the Driver Station.
    */
-  bool IsTest() const;
+  static bool IsTest();
 
   /**
    * Determine if the robot is current in Test mode and enabled.
@@ -203,7 +204,26 @@ class RobotBase {
    * @return True if the robot is currently operating in Test mode while
    * enabled as determined by the Driver Station.
    */
-  bool IsTestEnabled() const;
+  static bool IsTestEnabled();
+
+  /**
+   * Gets the currently selected operating mode of the driver station. Note this
+   * does not mean the robot is enabled; use IsEnabled() for that.
+   *
+   * @return the unique ID provided by the DriverStation::AddOpMode() function;
+   * may return 0 or a unique ID not added, so callers should be prepared to
+   * handle that case
+   */
+  static int64_t GetOpModeId();
+
+  /**
+   * Gets the currently selected operating mode of the driver station. Note this
+   * does not mean the robot is enabled; use IsEnabled() for that.
+   *
+   * @return Operating mode string; may return a string not in the list of
+   * options, so callers should be prepared to handle that case
+   */
+  static std::string GetOpMode();
 
   /**
    * Returns the main thread ID.

wpilibc/src/main/native/include/wpi/framework/RobotState.hpp

@@ -1,59 +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.
-
-#pragma once
-
-namespace wpi {
-
-/**
- * Robot state utility functions.
- */
-class RobotState {
- public:
-  RobotState() = delete;
-
-  /**
-   * Returns true if the robot is disabled.
-   *
-   * @return True if the robot is disabled.
-   */
-  static bool IsDisabled();
-
-  /**
-   * Returns true if the robot is enabled.
-   *
-   * @return True if the robot is enabled.
-   */
-  static bool IsEnabled();
-
-  /**
-   * Returns true if the robot is E-stopped.
-   *
-   * @return True if the robot is E-stopped.
-   */
-  static bool IsEStopped();
-
-  /**
-   * Returns true if the robot is in teleop mode.
-   *
-   * @return True if the robot is in teleop mode.
-   */
-  static bool IsTeleop();
-
-  /**
-   * Returns true if the robot is in autonomous mode.
-   *
-   * @return True if the robot is in autonomous mode.
-   */
-  static bool IsAutonomous();
-
-  /**
-   * Returns true if the robot is in test mode.
-   *
-   * @return True if the robot is in test mode.
-   */
-  static bool IsTest();
-};
-
-}  // namespace wpi

wpilibc/src/main/native/include/wpi/internal/DriverStationModeThread.hpp

@@ -7,6 +7,9 @@
 #include <atomic>
 #include <thread>
 
+#include "wpi/hal/DriverStationTypes.h"
+#include "wpi/util/Synchronization.h"
+
 namespace wpi::internal {
 /**
  * For internal use only.
@@ -15,8 +18,10 @@ class DriverStationModeThread {
  public:
   /**
    * For internal use only.
+   *
+   * @param word initial control word
    */
-  DriverStationModeThread();
+  explicit DriverStationModeThread(wpi::hal::ControlWord word);
 
   ~DriverStationModeThread();
 
@@ -30,44 +35,17 @@ class DriverStationModeThread {
    * Only to be used to tell the Driver Station what code you claim to be
    * executing for diagnostic purposes only.
    *
-   * @param entering If true, starting disabled code; if false, leaving disabled
-   * code
+   * @param word control word
    */
-  void InDisabled(bool entering);
-
-  /**
-   * Only to be used to tell the Driver Station what code you claim to be
-   * executing for diagnostic purposes only.
-   *
-   * @param entering If true, starting autonomous code; if false, leaving
-   * autonomous code
-   */
-  void InAutonomous(bool entering);
-
-  /**
-   * Only to be used to tell the Driver Station what code you claim to be
-   * executing for diagnostic purposes only.
-   *
-   * @param entering If true, starting teleop code; if false, leaving teleop
-   * code
-   */
-  void InTeleop(bool entering);
-
-  /**
-   * Only to be used to tell the Driver Station what code you claim to be
-   * executing for diagnostic purposes only.
-   *
-   * @param entering If true, starting test code; if false, leaving test code
-   */
-  void InTest(bool entering);
+  void InControl(wpi::hal::ControlWord word) {
+    m_userControlWord = word.GetValue().value;
+  }
 
  private:
   std::atomic_bool m_keepAlive{false};
+  wpi::util::Event m_event{false, false};
   std::thread m_thread;
   void Run();
-  bool m_userInDisabled{false};
-  bool m_userInAutonomous{false};
-  bool m_userInTeleop{false};
-  bool m_userInTest{false};
+  std::atomic<int64_t> m_userControlWord;
 };
 }  // namespace wpi::internal

wpilibc/src/main/native/include/wpi/opmode/LinearOpMode.hpp

@@ -0,0 +1,68 @@
+// 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 <stdint.h>
+
+#include <atomic>
+
+#include "wpi/opmode/OpMode.hpp"
+
+namespace wpi {
+
+/**
+ * An opmode structure for "linear" operation. The user is responsible for
+ * implementing any looping behavior; after Run() returns it will not be called
+ * again on the same object.
+ *
+ * Lifecycle:
+ *
+ * - Constructed when opmode selected on driver station
+ *
+ * - DisabledPeriodic() called periodically as long as DS is disabled
+ *
+ * - When DS transitions from disabled to enabled, Run() is called exactly once
+ *
+ * - When DS transitions from enabled to disabled, or a different opmode is
+ *   selected on the driver station, object is destroyed and not reused
+ *
+ * The user is responsible for exiting Run() when the opmode is directed to stop
+ * executing. This is indicated by IsRunning() returning false. All other
+ * methods should be written to return as quickly as possible when IsRunning()
+ * returns false.
+ */
+class LinearOpMode : public OpMode {
+ public:
+  /**
+   * Called periodically while the opmode is selected on the DS and the robot is
+   * disabled.
+   */
+  void DisabledPeriodic() override {}
+
+  /**
+   * Called once when the robot is enabled. When it returns, it will not be
+   * called again on the same object.
+   */
+  virtual void Run() = 0;
+
+  /**
+   * Returns true while this opmode is selected (regardless of enable state).
+   * All other functions should be written to return as quickly as possible when
+   * this returns false.
+   *
+   * @return True if opmode selected, false otherwise
+   */
+  bool IsRunning() const { return m_running; }
+
+  // implements OpMode interface
+  void OpModeRun(int64_t opModeId) final;
+
+  void OpModeStop() final;
+
+ private:
+  std::atomic_bool m_running{true};
+};
+
+}  // namespace wpi

wpilibc/src/main/native/include/wpi/opmode/OpMode.hpp

@@ -0,0 +1,45 @@
+// 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 <stdint.h>
+
+namespace wpi {
+
+/**
+ * Top-level interface for opmode classes. Users should generally extend one of
+ * the abstract implementations of this interface (e.g. PeriodicOpMode or
+ * LinearOpMode) rather than directly implementing this interface.
+ */
+class OpMode {
+ public:
+  /**
+   * The object is destroyed when the opmode is no longer selected on the DS or
+   * after OpModeRun() returns.
+   */
+  virtual ~OpMode() = default;
+
+  /**
+   * This function is called periodically while the opmode is selected on the DS
+   * (robot is disabled). Code that should only run once when the opmode is
+   * selected should go in the opmode constructor.
+   */
+  virtual void DisabledPeriodic() {}
+
+  /**
+   * This function is called when the opmode starts (robot is enabled).
+   *
+   * @param opModeId opmode unique ID
+   */
+  virtual void OpModeRun(int64_t opModeId) = 0;
+
+  /**
+   * This function is called asynchronously when the robot is disabled, to
+   * request the opmode return from OpModeRun().
+   */
+  virtual void OpModeStop() = 0;
+};
+
+}  // namespace wpi

wpilibc/src/main/native/include/wpi/opmode/PeriodicOpMode.hpp

@@ -0,0 +1,178 @@
+// 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 <stdint.h>
+
+#include <chrono>
+#include <functional>
+#include <vector>
+
+#include "wpi/hal/Notifier.h"
+#include "wpi/hal/Types.h"
+#include "wpi/opmode/OpMode.hpp"
+#include "wpi/system/Watchdog.hpp"
+#include "wpi/units/time.hpp"
+#include "wpi/util/priority_queue.hpp"
+
+namespace wpi {
+
+/**
+ * An opmode structure for periodic operation. This base class implements a loop
+ * that runs one or more functions periodically (on a set time interval aka loop
+ * period). The primary periodic callback function is the Periodic() function;
+ * the time interval for this callback is 20 ms by default, but may be changed
+ * via passing a different time interval to the constructor. Additional periodic
+ * callbacks with different intervals can be added using the AddPeriodic() set
+ * of functions.
+ *
+ * Lifecycle:
+ *
+ * - Constructed when opmode selected on driver station
+ *
+ * - DisabledPeriodic() called periodically as long as DS is disabled. Note
+ *   this is not called on a set time interval (it does not use the same time
+ *   interval as Periodic())
+ *
+ * - When DS transitions from disabled to enabled, Start() is called once
+ *
+ * - While DS is enabled, Periodic() is called periodically on the time interval
+ *   set by the constructor, and additional periodic callbacks added via
+ *   AddPeriodic() are called periodically on their set time intervals
+ *
+ * - When DS transitions from enabled to disabled, or a different opmode is
+ *   selected on the driver station when the DS is enabled, End() is called,
+ *   followed by the object being destroyed; the object is not reused
+ *
+ * - If a different opmode is selected on the driver station when the DS is
+ *   disabled, the object is destroyed (without End() being called); the object
+ *   is not reused
+ */
+class PeriodicOpMode : public OpMode {
+ public:
+  /** Default loop period. */
+  static constexpr auto kDefaultPeriod = 20_ms;
+
+ protected:
+  /**
+   * Constructor. Periodic opmodes may specify the period used for the
+   * Periodic() function.
+   *
+   * @param period period for callbacks to the Periodic() function
+   */
+  explicit PeriodicOpMode(wpi::units::second_t period = kDefaultPeriod);
+
+ public:
+  ~PeriodicOpMode() override;
+
+  /**
+   * Called periodically while the opmode is selected on the DS (robot is
+   * disabled).
+   */
+  void DisabledPeriodic() override {}
+
+  /**
+   * Called a single time when the robot transitions from disabled to enabled.
+   * This is called prior to Periodic() being called.
+   */
+  virtual void Start() {}
+
+  /** Called periodically while the robot is enabled. */
+  virtual void Periodic() = 0;
+
+  /**
+   * Called a single time when the robot transitions from enabled to disabled,
+   * or just before the destructor is called if a different opmode is selected
+   * while the robot is enabled.
+   */
+  virtual void End() {}
+
+  /**
+   * Return the system clock time in microseconds for the start of the current
+   * periodic loop. This is in the same time base as Timer.getFPGATimestamp(),
+   * but is stable through a loop. It is updated at the beginning of every
+   * periodic callback (including the normal periodic loop).
+   *
+   * @return Robot running time in microseconds, as of the start of the current
+   * periodic function.
+   */
+  int64_t GetLoopStartTime() const { return m_loopStartTimeUs; }
+
+  /**
+   * Add a callback to run at a specific period with a starting time offset.
+   *
+   * This is scheduled on the same Notifier as Periodic(), so Periodic() and the
+   * callback run synchronously. Interactions between them are thread-safe.
+   *
+   * @param callback The callback to run.
+   * @param period   The period at which to run the callback.
+   * @param offset   The offset from the common starting time. This is useful
+   *                 for scheduling a callback in a different timeslot relative
+   *                 to TimedRobot.
+   */
+  void AddPeriodic(std::function<void()> callback, wpi::units::second_t period,
+                   wpi::units::second_t offset = 0_s);
+
+  /**
+   * Gets time period between calls to Periodic() functions.
+   */
+  wpi::units::second_t GetPeriod() const { return m_period; }
+
+  /**
+   * Prints list of epochs added so far and their times.
+   */
+  void PrintWatchdogEpochs();
+
+ protected:
+  /** Loop function. */
+  void LoopFunc();
+
+ public:
+  // implements OpMode interface
+  void OpModeRun(int64_t opModeId) final;
+
+  void OpModeStop() final;
+
+ private:
+  class Callback {
+   public:
+    std::function<void()> func;
+    std::chrono::microseconds period;
+    std::chrono::microseconds expirationTime;
+
+    /**
+     * Construct a callback container.
+     *
+     * @param func      The callback to run.
+     * @param startTime The common starting point for all callback scheduling.
+     * @param period    The period at which to run the callback.
+     * @param offset    The offset from the common starting time.
+     */
+    Callback(std::function<void()> func, std::chrono::microseconds startTime,
+             std::chrono::microseconds period,
+             std::chrono::microseconds offset);
+
+    bool operator>(const Callback& rhs) const {
+      return expirationTime > rhs.expirationTime;
+    }
+  };
+
+  int64_t m_opModeId;
+  bool m_running = true;
+
+  wpi::hal::Handle<HAL_NotifierHandle, HAL_DestroyNotifier> m_notifier;
+  std::chrono::microseconds m_startTime;
+  int64_t m_loopStartTimeUs = 0;
+  wpi::units::second_t m_period;
+  Watchdog m_watchdog;
+
+  wpi::util::priority_queue<Callback, std::vector<Callback>,
+                            std::greater<Callback>>
+      m_callbacks;
+
+  void PrintLoopOverrunMessage();
+};
+
+}  // namespace wpi

wpilibc/src/main/native/include/wpi/simulation/CallbackStore.hpp

@@ -5,8 +5,10 @@
 #pragma once
 
 #include <functional>
+#include <span>
 #include <string_view>
 
+#include "wpi/hal/DriverStationTypes.h"
 #include "wpi/hal/Value.h"
 
 namespace wpi::sim {
@@ -14,6 +16,8 @@ namespace wpi::sim {
 using NotifyCallback = std::function<void(std::string_view, const HAL_Value*)>;
 using ConstBufferCallback = std::function<void(
     std::string_view, const unsigned char* buffer, unsigned int count)>;
+using OpModeOptionsCallback =
+    std::function<void(std::string_view, std::span<const HAL_OpModeOption>)>;
 using CancelCallbackFunc = void (*)(int32_t index, int32_t uid);
 using CancelCallbackNoIndexFunc = void (*)(int32_t uid);
 using CancelCallbackChannelFunc = void (*)(int32_t index, int32_t channel,
@@ -23,6 +27,9 @@ void CallbackStoreThunk(const char* name, void* param, const HAL_Value* value);
 void ConstBufferCallbackStoreThunk(const char* name, void* param,
                                    const unsigned char* buffer,
                                    unsigned int count);
+void OpModeOptionsCallbackStoreThunk(const char* name, void* param,
+                                     const HAL_OpModeOption* opmodes,
+                                     int32_t count);
 
 /**
  * Manages simulation callbacks; each object is associated with a callback.
@@ -46,6 +53,9 @@ class CallbackStore {
   CallbackStore(int32_t i, int32_t c, int32_t u, ConstBufferCallback cb,
                 CancelCallbackChannelFunc ccf);
 
+  CallbackStore(int32_t u, OpModeOptionsCallback cb,
+                CancelCallbackNoIndexFunc ccf);
+
   CallbackStore(const CallbackStore&) = delete;
   CallbackStore& operator=(const CallbackStore&) = delete;
 
@@ -60,6 +70,10 @@ class CallbackStore {
                                             const unsigned char* buffer,
                                             unsigned int count);
 
+  friend void OpModeOptionsCallbackStoreThunk(const char* name, void* param,
+                                              const HAL_OpModeOption* opmodes,
+                                              int32_t count);
+
  private:
   int32_t index;
   int32_t channel;
@@ -67,6 +81,7 @@ class CallbackStore {
 
   NotifyCallback callback;
   ConstBufferCallback constBufferCallback;
+  OpModeOptionsCallback opModeOptionsCallback;
   union {
     CancelCallbackFunc ccf;
     CancelCallbackChannelFunc cccf;

wpilibc/src/main/native/include/wpi/simulation/DriverStationSim.hpp

@@ -4,14 +4,46 @@
 
 #pragma once
 
+#include <stdint.h>
+
 #include <memory>
 
 #include "wpi/driverstation/DriverStation.hpp"
 #include "wpi/hal/DriverStationTypes.h"
+#include "wpi/hal/simulation/DriverStationData.h"
 #include "wpi/simulation/CallbackStore.hpp"
 
 namespace wpi::sim {
 
+class OpModeOptions : public std::span<HAL_OpModeOption> {
+ public:
+  OpModeOptions() = default;
+  OpModeOptions(HAL_OpModeOption* options, int32_t len)
+      : span{options, options + len} {}
+  OpModeOptions(const OpModeOptions&) = delete;
+
+  OpModeOptions(OpModeOptions&& oth) : span{oth} {
+    static_cast<span&>(oth) = {};
+  }
+
+  OpModeOptions& operator=(const OpModeOptions&) = delete;
+
+  OpModeOptions& operator=(OpModeOptions&& oth) {
+    if (data()) {
+      HALSIM_FreeOpModeOptionsArray(data(), size());
+    }
+    static_cast<span&>(*this) = oth;
+    static_cast<span&>(oth) = {};
+    return *this;
+  }
+
+  ~OpModeOptions() {
+    if (data()) {
+      HALSIM_FreeOpModeOptionsArray(data(), size());
+    }
+  }
+};
+
 /**
  * Class to control a simulated driver station.
  */
@@ -44,56 +76,29 @@ class DriverStationSim {
   static void SetEnabled(bool enabled);
 
   /**
-   * Register a callback on whether the DS is in autonomous mode.
+   * Register a callback on DS robot mode changes.
    *
-   * @param callback the callback that will be called on autonomous mode
-   *                 entrance/exit
+   * @param callback the callback that will be called when robot mode changes
    * @param initialNotify if true, the callback will be run on the initial value
    * @return the CallbackStore object associated with this callback
    */
   [[nodiscard]]
-  static std::unique_ptr<CallbackStore> RegisterAutonomousCallback(
+  static std::unique_ptr<CallbackStore> RegisterRobotModeCallback(
       NotifyCallback callback, bool initialNotify);
 
   /**
-   * Check if the DS is in autonomous.
+   * Get the robot mode set by the DS.
    *
-   * @return true if autonomous
+   * @return robot mode
    */
-  static bool GetAutonomous();
+  static HAL_RobotMode GetRobotMode();
 
   /**
-   * Change whether the DS is in autonomous.
+   * Change the robot mode set by the DS.
    *
-   * @param autonomous the new value
+   * @param robotMode the new value
    */
-  static void SetAutonomous(bool autonomous);
-
-  /**
-   * Register a callback on whether the DS is in test mode.
-   *
-   * @param callback the callback that will be called whenever the test mode
-   *                 is entered or left
-   * @param initialNotify if true, the callback will be run on the initial value
-   * @return the CallbackStore object associated with this callback
-   */
-  [[nodiscard]]
-  static std::unique_ptr<CallbackStore> RegisterTestCallback(
-      NotifyCallback callback, bool initialNotify);
-
-  /**
-   * Check if the DS is in test.
-   *
-   * @return true if test
-   */
-  static bool GetTest();
-
-  /**
-   * Change whether the DS is in test.
-   *
-   * @param test the new value
-   */
-  static void SetTest(bool test);
+  static void SetRobotMode(HAL_RobotMode robotMode);
 
   /**
    * Register a callback on the eStop state.
@@ -225,6 +230,50 @@ class DriverStationSim {
    */
   static void SetMatchTime(double matchTime);
 
+  /**
+   * Register a callback on DS opmode changes.
+   *
+   * @param callback the callback that will be called when opmode changes
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  static std::unique_ptr<CallbackStore> RegisterOpModeCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Get the opmode set by the DS.
+   *
+   * @return opmode
+   */
+  static int64_t GetOpMode();
+
+  /**
+   * Change the opmode set by the DS.
+   *
+   * @param opmode the new value
+   */
+  static void SetOpMode(int64_t opmode);
+
+  /**
+   * Register a callback on opmode options changes.
+   *
+   * @param callback the callback that will be called when the list of opmodes
+   * changes
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the CallbackStore object associated with this callback.
+   */
+  [[nodiscard]]
+  static std::unique_ptr<CallbackStore> RegisterOpModeOptionsCallback(
+      OpModeOptionsCallback callback, bool initialNotify);
+
+  /**
+   * Gets the list of opmode options.
+   *
+   * @return opmodes list
+   */
+  static OpModeOptions GetOpModeOptions();
+
   /**
    * Updates DriverStation data so that new values are visible to the user
    * program.

wpilibc/src/main/native/include/wpi/simulation/SimHooks.hpp

@@ -6,6 +6,7 @@
 
 #include <stdint.h>
 
+#include "wpi/hal/DriverStationTypes.h"
 #include "wpi/hal/HALBase.h"
 #include "wpi/units/time.hpp"
 
@@ -37,6 +38,20 @@ void SetProgramStarted(bool started);
  */
 bool GetProgramStarted();
 
+/**
+ * Sets the user program state (control word).
+ *
+ * @param controlWord control word
+ */
+void SetProgramState(wpi::hal::ControlWord controlWord);
+
+/**
+ * Gets the user program state (control word).
+ *
+ * @return Control word
+ */
+wpi::hal::ControlWord GetProgramState();
+
 /**
  * Restart the simulator time.
  */

wpilibc/src/main/python/pyproject.toml

@@ -94,7 +94,6 @@ MotorControllerGroup = "rpy/MotorControllerGroup.h"
 Notifier = "rpy/Notifier.h"
 
 # wpi/driverstation
-DSControlWord = "wpi/driverstation/DSControlWord.hpp"
 DriverStation = "wpi/driverstation/DriverStation.hpp"
 Gamepad = "wpi/driverstation/Gamepad.hpp"
 GenericHID = "wpi/driverstation/GenericHID.hpp"
@@ -106,8 +105,8 @@ XboxController = "wpi/driverstation/XboxController.hpp"
 
 # wpi/framework
 IterativeRobotBase = "wpi/framework/IterativeRobotBase.hpp"
+OpModeRobot = "wpi/framework/OpModeRobot.hpp"
 RobotBase = "wpi/framework/RobotBase.hpp"
-RobotState = "wpi/framework/RobotState.hpp"
 TimedRobot = "wpi/framework/TimedRobot.hpp"
 TimesliceRobot = "wpi/framework/TimesliceRobot.hpp"
 
@@ -182,6 +181,11 @@ Encoder = "wpi/hardware/rotation/Encoder.hpp"
 # wpi/internal
 DriverStationModeThread = "wpi/internal/DriverStationModeThread.hpp"
 
+# wpi/opmode
+LinearOpMode = "wpi/opmode/LinearOpMode.hpp"
+OpMode = "wpi/opmode/OpMode.hpp"
+PeriodicOpMode = "wpi/opmode/PeriodicOpMode.hpp"
+
 # wpi/smartdashboard
 Field2d = "wpi/smartdashboard/Field2d.hpp"
 FieldObject2d = "wpi/smartdashboard/FieldObject2d.hpp"

wpilibc/src/main/python/semiwrap/DSControlWord.yml

@@ -1,24 +0,0 @@
-classes:
-  wpi::DSControlWord:
-    methods:
-      DSControlWord:
-      IsEnabled:
-        no_release_gil: true
-      IsDisabled:
-        no_release_gil: true
-      IsEStopped:
-        no_release_gil: true
-      IsAutonomous:
-        no_release_gil: true
-      IsAutonomousEnabled:
-        no_release_gil: true
-      IsTeleop:
-        no_release_gil: true
-      IsTeleopEnabled:
-        no_release_gil: true
-      IsTest:
-        no_release_gil: true
-      IsDSAttached:
-        no_release_gil: true
-      IsFMSAttached:
-        no_release_gil: true

wpilibc/src/main/python/semiwrap/DriverStation.yml

@@ -1,6 +1,6 @@
 extra_includes:
-- rpy/ControlWord.h
 - wpi/datalog/DataLog.hpp
+- wpi/util/Color.hpp
 
 classes:
   wpi::DriverStation:
@@ -57,21 +57,23 @@ classes:
       GetStickPOVsAvailable:
       GetStickButtonsMaximumIndex:
       GetStickButtonsAvailable:
+      GetRobotMode:
+      AddOpMode:
+        overloads:
+          RobotMode, std::string_view, std::string_view, std::string_view:
+          RobotMode, std::string_view, std::string_view, std::string_view, const wpi::util::Color&, const wpi::util::Color&:
+      RemoveOpMode:
+      PublishOpModes:
+      ClearOpModes:
+      GetOpModeId:
+      GetOpMode:
+      IsOpMode:
+        overloads:
+          int64_t:
+          std::string_view:
+      GetControlWord:
       GetStickTouchpadFinger:
       GetStickTouchpadFingerAvailable:
-    inline_code: |
-      .def("getControlState",
-        [](DriverStation *self) -> std::tuple<bool, bool, bool> {
-          py::gil_scoped_release release;
-          return rpy::GetControlState();
-        },
-        py::doc("More efficient way to determine what state the robot is in.\n"
-                "\n"
-                ":returns: booleans representing enabled, isautonomous, istest\n"
-                "\n"
-                ".. versionadded:: 2019.2.1\n"
-                "\n"
-                ".. note:: This function only exists in RobotPy\n"))
   wpi::DriverStation::TouchpadFinger:
     attributes:
       down:

wpilibc/src/main/python/semiwrap/DriverStationModeThread.yml

@@ -3,7 +3,4 @@ classes:
     rename: _DriverStationModeThread
     methods:
       DriverStationModeThread:
-      InAutonomous:
-      InDisabled:
-      InTeleop:
-      InTest:
+      InControl:

wpilibc/src/main/python/semiwrap/LinearOpMode.yml

@@ -0,0 +1,8 @@
+classes:
+  wpi::LinearOpMode:
+    methods:
+      DisabledPeriodic:
+      Run:
+      IsRunning:
+      OpModeRun:
+      OpModeStop:

wpilibc/src/main/python/semiwrap/OpMode.yml

@@ -0,0 +1,6 @@
+classes:
+  wpi::OpMode:
+    methods:
+      DisabledPeriodic:
+      OpModeRun:
+      OpModeStop:

wpilibc/src/main/python/semiwrap/OpModeRobot.yml

@@ -0,0 +1,26 @@
+extra_includes:
+- wpi/util/Color.hpp
+
+classes:
+  wpi::OpModeRobotBase:
+    methods:
+      StartCompetition:
+      EndCompetition:
+      OpModeRobotBase:
+      DriverStationConnected:
+      NonePeriodic:
+      AddOpModeFactory:
+        overloads:
+          OpModeFactory, RobotMode, std::string_view, std::string_view, std::string_view:
+          ? OpModeFactory, RobotMode, std::string_view, std::string_view, std::string_view, const wpi::util::Color&, const wpi::util::Color&
+          :
+      RemoveOpMode:
+      PublishOpModes:
+      ClearOpModes:
+  wpi::OpModeRobot:
+    ignore: true
+    methods:
+      AddOpMode:
+        overloads:
+          RobotMode, std::string_view, std::string_view, std::string_view, const wpi::util::Color&, const wpi::util::Color&:
+          RobotMode, std::string_view, std::string_view, std::string_view:

wpilibc/src/main/python/semiwrap/PeriodicOpMode.yml

@@ -0,0 +1,17 @@
+classes:
+  wpi::PeriodicOpMode:
+    attributes:
+      kDefaultPeriod:
+    methods:
+      DisabledPeriodic:
+      Start:
+      Periodic:
+      End:
+      GetLoopStartTime:
+      AddPeriodic:
+      GetPeriod:
+      PrintWatchdogEpochs:
+      OpModeRun:
+      OpModeStop:
+      PeriodicOpMode:
+      LoopFunc:

wpilibc/src/main/python/semiwrap/RobotBase.yml

@@ -1,6 +1,5 @@
 extra_includes:
 - wpi/driverstation/DriverStation.hpp
-- rpy/ControlWord.h
 
 functions:
   # TODO
@@ -36,6 +35,8 @@ classes:
       IsReal:
       IsSimulation:
       RobotBase:
+      GetOpModeId:
+      GetOpMode:
     inline_code: |
       .def_static("main",
         [](py::object robot_cls) -> py::object {
@@ -43,20 +44,7 @@ classes:
           auto starter = start.attr("RobotStarter")();
           return starter.attr("run")(robot_cls);
         },
-        py::arg("robot_cls"), py::doc("Starting point for the application"))
-      .def(
-        "getControlState",
-        [](RobotBase *self) -> std::tuple<bool, bool, bool> {
-          py::gil_scoped_release release;
-          return rpy::GetControlState();
-        },
-        py::doc("More efficient way to determine what state the robot is in.\n"
-                "\n"
-                ":returns: booleans representing enabled, isautonomous, istest\n"
-                "\n"
-                ".. versionadded:: 2019.2.1\n"
-                "\n"
-                ".. note:: This function only exists in RobotPy\n"));
+        py::arg("robot_cls"), py::doc("Starting point for the application"));
 
       auto logger = py::module::import("logging").attr("getLogger")("robot");
       cls_RobotBase.attr("logger") = logger;

wpilibc/src/main/python/semiwrap/RobotState.yml

@@ -1,10 +0,0 @@
-classes:
-  wpi::RobotState:
-    nodelete: true
-    methods:
-      IsDisabled:
-      IsEnabled:
-      IsEStopped:
-      IsTeleop:
-      IsAutonomous:
-      IsTest:

wpilibc/src/main/python/semiwrap/simulation/CallbackStore.yml

@@ -3,6 +3,8 @@ functions:
     ignore: true
   ConstBufferCallbackStoreThunk:
     ignore: true
+  OpModeOptionsCallbackStoreThunk:
+    ignore: true
 classes:
   wpi::sim::CallbackStore:
     force_type_casters:
@@ -23,4 +25,6 @@ classes:
             ignore: true
           int32_t, int32_t, int32_t, ConstBufferCallback, CancelCallbackChannelFunc:
             ignore: true
+          int32_t, OpModeOptionsCallback, CancelCallbackNoIndexFunc:
+            ignore: true
       SetUid:

wpilibc/src/main/python/semiwrap/simulation/DriverStationSim.yml

@@ -1,4 +1,6 @@
 classes:
+  wpi::sim::OpModeOptions:
+    ignore: true
   wpi::sim::DriverStationSim:
     force_type_casters:
     - std::function
@@ -6,12 +8,6 @@ classes:
       RegisterEnabledCallback:
       GetEnabled:
       SetEnabled:
-      RegisterAutonomousCallback:
-      GetAutonomous:
-      SetAutonomous:
-      RegisterTestCallback:
-      GetTest:
-      SetTest:
       RegisterEStopCallback:
       GetEStop:
       SetEStop:
@@ -51,3 +47,12 @@ classes:
       SetJoystickPOVsAvailable:
       SetJoystickButtonsMaximumIndex:
       SetJoystickButtonsAvailable:
+      RegisterRobotModeCallback:
+      GetRobotMode:
+      SetRobotMode:
+      RegisterOpModeCallback:
+      GetOpMode:
+      SetOpMode:
+      RegisterOpModeOptionsCallback:
+      GetOpModeOptions:
+        ignore: true

wpilibc/src/main/python/semiwrap/simulation/SimHooks.yml

@@ -3,6 +3,8 @@ functions:
   WaitForProgramStart:
   SetProgramStarted:
   GetProgramStarted:
+  SetProgramState:
+  GetProgramState:
   RestartTiming:
   PauseTiming:
   ResumeTiming:

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

wpilibc/src/test/native/cpp/OpModeRobotTest.cpp

@@ -0,0 +1,175 @@
+// 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 <sys/types.h>
+
+#include <gtest/gtest.h>
+
+#include "wpi/simulation/DriverStationSim.hpp"
+#include "wpi/simulation/SimHooks.hpp"
+#include "wpi/util/Color.hpp"
+
+namespace {
+class OpModeRobotTest : public ::testing::Test {
+ protected:
+  void SetUp() override {
+    wpi::sim::PauseTiming();
+    wpi::sim::SetProgramStarted(false);
+  }
+
+  void TearDown() override { wpi::sim::ResumeTiming(); }
+};
+
+class MockRobot;
+
+class MockOpMode : public wpi::OpMode {
+ public:
+  std::atomic<uint32_t> m_disabledPeriodicCount{0};
+  std::atomic<uint32_t> m_opModeRunCount{0};
+  std::atomic<uint32_t> m_opModeStopCount{0};
+
+  MockOpMode() = default;
+  void DisabledPeriodic() override { m_disabledPeriodicCount++; }
+  void OpModeRun(int64_t opModeId) override { m_opModeRunCount++; }
+  void OpModeStop() override { m_opModeStopCount++; }
+};
+
+class OneArgOpMode : public wpi::OpMode {
+ public:
+  explicit OneArgOpMode(MockRobot& robot) {}
+  void OpModeRun(int64_t opModeId) override {}
+  void OpModeStop() override {}
+};
+
+class MockRobot : public wpi::OpModeRobot<MockRobot> {
+ public:
+  std::atomic<uint32_t> m_driverStationConnectedCount{0};
+  std::atomic<uint32_t> m_nonePeriodicCount{0};
+
+  MockRobot() = default;
+
+  void DriverStationConnected() override { m_driverStationConnectedCount++; }
+
+  void NonePeriodic() override { m_nonePeriodicCount++; }
+};
+}  // namespace
+
+static_assert(wpi::ConstructibleOpMode<MockOpMode, MockRobot>);
+static_assert(wpi::ConstructibleOpMode<OneArgOpMode, MockRobot>);
+
+TEST_F(OpModeRobotTest, AddOpMode) {
+  struct MyMockRobot : public MockRobot {
+    MyMockRobot() {
+      AddOpMode<MockOpMode>(wpi::RobotMode::AUTONOMOUS, "NoArgOpMode-Auto",
+                            "Group", "Description", wpi::util::Color::kWhite,
+                            wpi::util::Color::kBlack);
+      AddOpMode<OneArgOpMode>(wpi::RobotMode::TEST, "OneArgOpMode-Test",
+                              "Group", "Description", wpi::util::Color::kWhite,
+                              wpi::util::Color::kBlack);
+      AddOpMode<MockOpMode>(wpi::RobotMode::TELEOPERATED, "NoArgOpMode");
+      AddOpMode<OneArgOpMode>(wpi::RobotMode::TELEOPERATED, "OneArgOpMode");
+      PublishOpModes();
+    }
+  };
+  MyMockRobot robot;
+
+  auto options = wpi::sim::DriverStationSim::GetOpModeOptions();
+  ASSERT_EQ(options.size(), 4u);
+  int indexes[4] = {-1, -1, -1, -1};
+  for (size_t i = 0; i < options.size(); ++i) {
+    auto name = wpi::util::to_string_view(&options[i].name);
+    if (name == "NoArgOpMode-Auto") {
+      indexes[0] = i;
+    } else if (name == "OneArgOpMode-Test") {
+      indexes[1] = i;
+    } else if (name == "NoArgOpMode") {
+      indexes[2] = i;
+    } else if (name == "OneArgOpMode") {
+      indexes[3] = i;
+    }
+  }
+
+  int i = indexes[0];
+  ASSERT_NE(i, -1);
+  EXPECT_EQ(wpi::util::to_string_view(&options[i].group), "Group");
+  EXPECT_EQ(wpi::util::to_string_view(&options[i].description), "Description");
+  EXPECT_EQ(options[i].textColor, 0xffffff);
+  EXPECT_EQ(options[i].backgroundColor, 0x000000);
+
+  i = indexes[1];
+  ASSERT_NE(i, -1);
+  EXPECT_EQ(wpi::util::to_string_view(&options[i].group), "Group");
+  EXPECT_EQ(wpi::util::to_string_view(&options[i].description), "Description");
+  EXPECT_EQ(options[i].textColor, 0xffffff);
+  EXPECT_EQ(options[i].backgroundColor, 0x000000);
+
+  i = indexes[2];
+  ASSERT_NE(i, -1);
+  EXPECT_EQ(wpi::util::to_string_view(&options[i].group), "");
+  EXPECT_EQ(wpi::util::to_string_view(&options[i].description), "");
+  EXPECT_EQ(options[i].textColor, -1);
+  EXPECT_EQ(options[i].backgroundColor, -1);
+
+  i = indexes[3];
+  ASSERT_NE(i, -1);
+  EXPECT_EQ(wpi::util::to_string_view(&options[i].group), "");
+  EXPECT_EQ(wpi::util::to_string_view(&options[i].description), "");
+  EXPECT_EQ(options[i].textColor, -1);
+  EXPECT_EQ(options[i].backgroundColor, -1);
+}
+
+TEST_F(OpModeRobotTest, ClearOpModes) {
+  struct MyMockRobot : public MockRobot {
+    MyMockRobot() {
+      AddOpMode<MockOpMode>(wpi::RobotMode::TELEOPERATED, "NoArgOpMode");
+      AddOpMode<OneArgOpMode>(wpi::RobotMode::TELEOPERATED, "OneArgOpMode");
+      PublishOpModes();
+    }
+  };
+  MyMockRobot robot;
+
+  robot.ClearOpModes();
+  auto options = wpi::sim::DriverStationSim::GetOpModeOptions();
+  EXPECT_TRUE(options.empty());
+}
+
+TEST_F(OpModeRobotTest, RemoveOpMode) {
+  struct MyMockRobot : public MockRobot {
+    MyMockRobot() {
+      AddOpMode<MockOpMode>(wpi::RobotMode::TELEOPERATED, "NoArgOpMode");
+      AddOpMode<OneArgOpMode>(wpi::RobotMode::TELEOPERATED, "OneArgOpMode");
+      PublishOpModes();
+    }
+  };
+  MyMockRobot robot;
+
+  robot.RemoveOpMode(wpi::RobotMode::TELEOPERATED, "NoArgOpMode");
+  robot.PublishOpModes();
+  auto options = wpi::sim::DriverStationSim::GetOpModeOptions();
+  ASSERT_EQ(options.size(), 1u);
+  EXPECT_EQ(wpi::util::to_string_view(&options[0].name), "OneArgOpMode");
+}
+
+TEST_F(OpModeRobotTest, NonePeriodic) {
+  struct MyMockRobot : public MockRobot {
+    MyMockRobot() {
+      AddOpMode<MockOpMode>(wpi::RobotMode::TELEOPERATED, "NoArgOpMode");
+      AddOpMode<OneArgOpMode>(wpi::RobotMode::TELEOPERATED, "OneArgOpMode");
+      PublishOpModes();
+    }
+  };
+  MyMockRobot robot;
+
+  std::thread robotThread{[&] { robot.StartCompetition(); }};
+  wpi::sim::WaitForProgramStart();
+
+  // Time step to get periodic calls on 50 ms timeout
+  wpi::sim::StepTiming(110_ms);
+  EXPECT_EQ(robot.m_nonePeriodicCount.load(), 2u);
+
+  robot.EndCompetition();
+  robotThread.join();
+}

wpilibc/src/test/native/cpp/TimedRobotTest.cpp

@@ -11,6 +11,7 @@
 
 #include <gtest/gtest.h>
 
+#include "wpi/hal/DriverStationTypes.h"
 #include "wpi/simulation/DriverStationSim.hpp"
 #include "wpi/simulation/SimHooks.hpp"
 
@@ -161,8 +162,7 @@ TEST_F(TimedRobotTest, AutonomousMode) {
   wpi::sim::WaitForProgramStart();
 
   wpi::sim::DriverStationSim::SetEnabled(true);
-  wpi::sim::DriverStationSim::SetAutonomous(true);
-  wpi::sim::DriverStationSim::SetTest(false);
+  wpi::sim::DriverStationSim::SetRobotMode(HAL_ROBOTMODE_AUTONOMOUS);
   wpi::sim::DriverStationSim::NotifyNewData();
 
   EXPECT_EQ(1u, robot.m_simulationInitCount);
@@ -234,8 +234,7 @@ TEST_F(TimedRobotTest, TeleopMode) {
   wpi::sim::WaitForProgramStart();
 
   wpi::sim::DriverStationSim::SetEnabled(true);
-  wpi::sim::DriverStationSim::SetAutonomous(false);
-  wpi::sim::DriverStationSim::SetTest(false);
+  wpi::sim::DriverStationSim::SetRobotMode(HAL_ROBOTMODE_TELEOPERATED);
   wpi::sim::DriverStationSim::NotifyNewData();
 
   EXPECT_EQ(1u, robot.m_simulationInitCount);
@@ -306,8 +305,7 @@ TEST_F(TimedRobotTest, TestMode) {
   wpi::sim::WaitForProgramStart();
 
   wpi::sim::DriverStationSim::SetEnabled(true);
-  wpi::sim::DriverStationSim::SetAutonomous(false);
-  wpi::sim::DriverStationSim::SetTest(true);
+  wpi::sim::DriverStationSim::SetRobotMode(HAL_ROBOTMODE_TEST);
   wpi::sim::DriverStationSim::NotifyNewData();
 
   EXPECT_EQ(1u, robot.m_simulationInitCount);
@@ -369,8 +367,6 @@ TEST_F(TimedRobotTest, TestMode) {
   EXPECT_EQ(0u, robot.m_testExitCount);
 
   wpi::sim::DriverStationSim::SetEnabled(false);
-  wpi::sim::DriverStationSim::SetAutonomous(false);
-  wpi::sim::DriverStationSim::SetTest(false);
   wpi::sim::DriverStationSim::NotifyNewData();
   wpi::sim::StepTiming(20_ms);  // Wait for Notifiers
 
@@ -404,8 +400,6 @@ TEST_F(TimedRobotTest, ModeChange) {
 
   // Start in disabled
   wpi::sim::DriverStationSim::SetEnabled(false);
-  wpi::sim::DriverStationSim::SetAutonomous(false);
-  wpi::sim::DriverStationSim::SetTest(false);
   wpi::sim::DriverStationSim::NotifyNewData();
 
   EXPECT_EQ(0u, robot.m_disabledInitCount);
@@ -432,8 +426,7 @@ TEST_F(TimedRobotTest, ModeChange) {
 
   // Transition to autonomous
   wpi::sim::DriverStationSim::SetEnabled(true);
-  wpi::sim::DriverStationSim::SetAutonomous(true);
-  wpi::sim::DriverStationSim::SetTest(false);
+  wpi::sim::DriverStationSim::SetRobotMode(HAL_ROBOTMODE_AUTONOMOUS);
   wpi::sim::DriverStationSim::NotifyNewData();
 
   wpi::sim::StepTiming(kPeriod);
@@ -450,8 +443,7 @@ TEST_F(TimedRobotTest, ModeChange) {
 
   // Transition to teleop
   wpi::sim::DriverStationSim::SetEnabled(true);
-  wpi::sim::DriverStationSim::SetAutonomous(false);
-  wpi::sim::DriverStationSim::SetTest(false);
+  wpi::sim::DriverStationSim::SetRobotMode(HAL_ROBOTMODE_TELEOPERATED);
   wpi::sim::DriverStationSim::NotifyNewData();
 
   wpi::sim::StepTiming(kPeriod);
@@ -468,8 +460,7 @@ TEST_F(TimedRobotTest, ModeChange) {
 
   // Transition to test
   wpi::sim::DriverStationSim::SetEnabled(true);
-  wpi::sim::DriverStationSim::SetAutonomous(false);
-  wpi::sim::DriverStationSim::SetTest(true);
+  wpi::sim::DriverStationSim::SetRobotMode(HAL_ROBOTMODE_TEST);
   wpi::sim::DriverStationSim::NotifyNewData();
 
   wpi::sim::StepTiming(kPeriod);
@@ -486,8 +477,6 @@ TEST_F(TimedRobotTest, ModeChange) {
 
   // Transition to disabled
   wpi::sim::DriverStationSim::SetEnabled(false);
-  wpi::sim::DriverStationSim::SetAutonomous(false);
-  wpi::sim::DriverStationSim::SetTest(false);
   wpi::sim::DriverStationSim::NotifyNewData();
 
   wpi::sim::StepTiming(kPeriod);

wpilibc/src/test/native/cpp/simulation/DriverStationSimTest.cpp

@@ -12,7 +12,7 @@
 #include "callback_helpers/TestCallbackHelpers.hpp"
 #include "wpi/driverstation/DriverStation.hpp"
 #include "wpi/driverstation/Joystick.hpp"
-#include "wpi/framework/RobotState.hpp"
+#include "wpi/hal/DriverStationTypes.h"
 #include "wpi/simulation/SimHooks.hpp"
 
 using namespace wpi;
@@ -26,11 +26,11 @@ TEST(DriverStationTest, Enabled) {
   BooleanCallback callback;
   auto cb =
       DriverStationSim::RegisterEnabledCallback(callback.GetCallback(), false);
+  DriverStationSim::SetRobotMode(HAL_ROBOTMODE_TELEOPERATED);
   DriverStationSim::SetEnabled(true);
   DriverStationSim::NotifyNewData();
   EXPECT_TRUE(DriverStationSim::GetEnabled());
   EXPECT_TRUE(DriverStation::IsEnabled());
-  EXPECT_TRUE(RobotState::IsEnabled());
   EXPECT_TRUE(callback.WasTriggered());
   EXPECT_TRUE(callback.GetLastValue());
 }
@@ -40,16 +40,16 @@ TEST(DriverStationTest, AutonomousMode) {
   DriverStationSim::ResetData();
 
   EXPECT_FALSE(DriverStation::IsAutonomous());
-  BooleanCallback callback;
-  auto cb = DriverStationSim::RegisterAutonomousCallback(callback.GetCallback(),
-                                                         false);
-  DriverStationSim::SetAutonomous(true);
+  EnumCallback callback;
+  auto cb = DriverStationSim::RegisterRobotModeCallback(callback.GetCallback(),
+                                                        false);
+  DriverStationSim::SetRobotMode(HAL_ROBOTMODE_AUTONOMOUS);
   DriverStationSim::NotifyNewData();
-  EXPECT_TRUE(DriverStationSim::GetAutonomous());
+  EXPECT_EQ(DriverStationSim::GetRobotMode(), HAL_ROBOTMODE_AUTONOMOUS);
   EXPECT_TRUE(DriverStation::IsAutonomous());
-  EXPECT_TRUE(RobotState::IsAutonomous());
+  EXPECT_EQ(DriverStation::GetRobotMode(), RobotMode::AUTONOMOUS);
   EXPECT_TRUE(callback.WasTriggered());
-  EXPECT_TRUE(callback.GetLastValue());
+  EXPECT_EQ(callback.GetLastValue(), HAL_ROBOTMODE_AUTONOMOUS);
 }
 
 TEST(DriverStationTest, Mode) {
@@ -57,16 +57,16 @@ TEST(DriverStationTest, Mode) {
   DriverStationSim::ResetData();
 
   EXPECT_FALSE(DriverStation::IsTest());
-  BooleanCallback callback;
-  auto cb =
-      DriverStationSim::RegisterTestCallback(callback.GetCallback(), false);
-  DriverStationSim::SetTest(true);
+  EnumCallback callback;
+  auto cb = DriverStationSim::RegisterRobotModeCallback(callback.GetCallback(),
+                                                        false);
+  DriverStationSim::SetRobotMode(HAL_ROBOTMODE_TEST);
   DriverStationSim::NotifyNewData();
-  EXPECT_TRUE(DriverStationSim::GetTest());
+  EXPECT_EQ(DriverStationSim::GetRobotMode(), HAL_ROBOTMODE_TEST);
   EXPECT_TRUE(DriverStation::IsTest());
-  EXPECT_TRUE(RobotState::IsTest());
+  EXPECT_EQ(DriverStation::GetRobotMode(), RobotMode::TEST);
   EXPECT_TRUE(callback.WasTriggered());
-  EXPECT_TRUE(callback.GetLastValue());
+  EXPECT_EQ(callback.GetLastValue(), HAL_ROBOTMODE_TEST);
 }
 
 TEST(DriverStationTest, Estop) {
@@ -81,7 +81,6 @@ TEST(DriverStationTest, Estop) {
   DriverStationSim::NotifyNewData();
   EXPECT_TRUE(DriverStationSim::GetEStop());
   EXPECT_TRUE(DriverStation::IsEStopped());
-  EXPECT_TRUE(RobotState::IsEStopped());
   EXPECT_TRUE(callback.WasTriggered());
   EXPECT_TRUE(callback.GetLastValue());
 }