[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/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.
  */