diff --git a/design-docs/opmodes.md b/design-docs/opmodes.md index 92c6e2ae5a..3924cf7937 100644 --- a/design-docs/opmodes.md +++ b/design-docs/opmodes.md @@ -18,7 +18,7 @@ Primary use cases for operator-selectable code include: Notably these use cases span both matches and off-field testing. -Secondarily, some teams may want to use different code structures for different use cases (e.g. linear for autonomous and iterative for test or teleop). +Secondarily, some teams may want to use different code structures for different use cases. # Background @@ -114,11 +114,13 @@ Providing the top-level teleop, autonomous, and test selection available in the - DS provides drop-down selector(s) for the user-defined opmodes; these are filtered based on the top-level mode selector (e.g. if auto is selected, a single drop-down selector of just the auto opmodes is provided). For match mode or when FMS-attached, two drop downs are provided (one for auto opmode selection and one for teleop opmode selection). The drop-down selector provides grouped categories as specified by the robot program. -- Robot programs are structured to have a top-level Robot class (e.g. motors/sensors/subsystems); this is also where robot-wide initialization is performed (in the class constructor or static initializer block). The Robot class provides overrideable periodic type functions for users to run code when the robot is disabled. +- Robot programs are structured to have a top-level Robot class (e.g. motors/sensors/subsystems); this is also where robot-wide initialization is performed (in the class constructor or static initializer block). -- OpModes are usually registered via annotation of classes. These annotations may specify a name (or default to the class name), group name (for grouping of routines in the selection list), and description. Annotations are used to specify whether the class is a teleop, auto, or test opmode. +- The Robot class has access to comprehensive lifecycle methods that are called at different stages of robot operation, providing behavior that occurs across all opmodes. -- OpModes may also be registered via annotation of functions (in the Robot class only) or via explicit function calls. As C++ does not support annotations, function call registration is the only available method in that language. If manually registered, `publishOpModes()` must be called to publish the list of opmodes to the DS. +- OpModes are usually registered via annotation of classes. These annotations may specify a name (or default to the class name), group name (for grouping of routines in the selection list), description, and optional display colors. Annotations are used to specify whether the class is a teleop, auto, or test opmode. + +- OpModes may also be registered via explicit function calls. As C++ does not support annotations, function call registration is the only available method in that language. If manually registered, `publishOpModes()` must be called to publish the list of opmodes to the DS. - For maximum flexibility, all code in the robot project has access to the enable/disable state, the overall robot teleop/auto/test mode, and the selected opmodes for teleop, auto, and test (even when the robot is disabled). @@ -146,185 +148,147 @@ Java is used for illustrative purposes. Python and C++ should follow a similar ### OpModeRobot -The `OpModeRobot` class is the base class for the user's `Robot` class. It also implements the private library machinery for robot startup and robot execution (including creating and transitioning between opmodes in accordance with the opmode lifecycle, as described in the following section). +The `OpModeRobot` class is the base class for the user's `Robot` class. It extends `RobotBase` directly and implements the private library machinery for robot startup and robot execution (including creating and transitioning between opmodes in accordance with the opmode lifecycle, as described in the following section). ```java -public abstract class OpModeRobot { - public void nonePeriodic() { - // this code is called when no opmode is selected +public abstract class OpModeRobot extends RobotBase { + // OpMode registration methods + public void addOpModeFactory(Supplier factory, RobotMode mode, + String name, String group, String description, + Color textColor, Color backgroundColor) {...} + + // add a particular opmode class (Java only) + public void addOpMode(Class cls, RobotMode mode, + String name, String group, String description, + Color textColor, Color backgroundColor) {...} + public void addAnnotatedOpMode(Class cls) {...} + + // add all annotated opmodes in a package and nested packages + public void addAnnotatedOpModeClasses(Package pkg) {...} + + public void removeOpMode(RobotMode mode, String name) {...} + public void publishOpModes() {...} + public void clearOpModes() {...} + + // Robot lifecycle methods + public void driverStationConnected() { + // called once when the DS first connects } - // these functions allow users to add opmodes without annotations - public void addAutonomousOpMode(Supplier factory, String name, String group, String description) {...} - public void addTeleoperatedOpMode(Supplier factory, String name, String group, String description) {...} - public void addTestOpMode(Supplier factory, String name, String group, String description) {...} + public void robotPeriodic() { + // called periodically every loop, regardless of enabled state or opmode + } + + public void simulationInit() { + // called once during robot initialization in simulation + } + + public void simulationPeriodic() { + // called periodically in simulation + } + + public void disabledInit() { + // called once when the robot becomes disabled + } + + public void disabledPeriodic() { + // called periodically while the robot is disabled + } + + public void disabledExit() { + // called once when the robot exits disabled state + } + + public void nonePeriodic() { + // called periodically when no opmode is selected + } } ``` +`OpModeRobot` (in Java) automatically scans the user's package and subpackages for `@Autonomous`, `@Teleop`, and `@TestOpMode` classes in its constructor and publishes that list to the DS. When opmodes are added or removed manually after construction, `publishOpModes()` must be called to push changes to the DS. + ### OpMode Classes -There are library base classes for different coding styles of routines. The `OpMode` interface serves as the base interface for all opmodes. Most users will use either `LinearOpMode` or `PeriodicOpMode` abstract base class implementations of this interface, but this interface enables users to create more customized opmodes while still utilizing the core robot base class opmode-switching implementation. +The `OpMode` interface serves as the base interface for all opmodes. Most users will extend `PeriodicOpMode`, but users may also implement `OpMode` directly for custom behavior. -The full lifecycle of a opmode is as follows: -- Operator selects opmode on DS -> opmode object is constructed -- If different opmode is selected -> `opModeClose()` is called (which typically just calls `close()`), object is released to GC -- While opmode is selected and robot is disabled, `disabledPeriodic()` is called -- When the robot is enabled in the selected opmode, `opModeRun()` is called; the result of this is different for different opmode base classes: - - `start()` is called once (for `PeriodicOpMode`) - - `run()` is called (for `LinearOpMode`), or `periodic()` is called periodically (for `PeriodicOpMode`) -- When the robot is disabled, `opModeStop()` is called (which results in `end()` being called for `PeriodicOpMode`), followed by `opModeClose()` (which results in `close()` being called for both `PeriodicOpMode` and `LinearOpMode`), object is released to GC +The lifecycle of an opmode is: +- When operator selects opmode on DS, a new opmode object is constructed +- While selected and disabled, `disabledPeriodic()` is called +- On disabled → enabled transition, `start()` is called once +- While enabled, `periodic()` is called at `OpModeRobot#getPeriod()`, and additional callbacks from `getCallbacks()` are run at their own configured rates +- If robot disables or a different opmode is selected while enabled, `end()` is called then `close()` is called (Java), or the object is destroyed (C++/Python); the object is not reused +- If a different opmode is selected while disabled, only `close()` is called (Java), or the object is destroyed (C++); the object is not reused -Following `opModeClose()` being called, a *new* opmode object is constructed based on the DS teleop/auto/test/match selector and selected opmode. In teleop/auto/test, the drop-down selection will be the same as before the previous enable, so the same opmode class is constructed again. In match (or when FMS-connected), only the selected auto opmode object is initially constructed; once auto completes, the selected teleop opmode object is constructed. Thus only zero or one opmode objects will ever be "alive" at any given time. +Following `close()` being called (Java)/the opmode being destroyed (C++), a *new* opmode object is constructed based on the DS teleop/auto/test/match selector and selected opmode. In teleop/auto/test, the drop-down selection will be the same as before the previous enable, so the same opmode class is constructed again. In match (or when FMS-connected), only the selected auto opmode object is initially constructed; once auto completes, the selected teleop opmode object is constructed. Thus only zero or one opmode objects will ever be "alive" at any given time. -For consistency in operation, the library will ensure that `disabledPeriodic()` is always called at least once before `opModeRun()` is called. +For consistency in operation, the library will ensure that `disabledPeriodic()` is always called at least once before `start()` is called. -User implementations of opmode classes may have either a no-parameter constructor or a constructor that accepts the user's `Robot` class type. If available, the library will call the latter and pass the user's `Robot` object to it when constructing the class. - -The library will use escalating steps to attempt to terminate a opmode if `opModeRun()` does not return within a reasonable timeframe after `opModeStop()` is called, up to and including termination of the robot executable process (which will result in an automatic restart of it at the system level). User code (particularly in `LinearOpMode` derived classes) should check for `isRunning()` returning false and return as quickly as possible to allow the opmode to terminate gracefully. +User implementations of opmode classes may have either a no-parameter constructor or a constructor that accepts (a subclass of) the user's `Robot` class type. If available, the library will call the latter and pass the user's `Robot` object to it when constructing the class. ```java -public interface OpMode { - // this function is called periodically while the opmode is selected on the DS (robot is disabled) - // Note: it may be called once when the robot is enabled to ensure it's always called once before - // opModeRun() is called +public interface OpMode extends AutoCloseable { + // called periodically while selected and robot is disabled default void disabledPeriodic() {} - // this function is called when the opmode starts (robot is enabled) - void opModeRun(long opModeId) throws InterruptedException; + // called once when this opmode transitions to enabled + default void start() {} - // this function is called asynchronously when the robot is disabled, - // to request the opmode return from opModeRun() - void opModeStop(); + // called periodically while enabled at OpModeRobot#getPeriod() + default void periodic() {} - // this function is called when the opmode is de-selected on the DS or after - // opModeRun() returns - void opModeClose(); -} -``` - -```java -public abstract class LinearOpMode implements OpMode { - // the class is constructed when the opmode is selected on the DS + // called asynchronously when robot disables or switches opmodes while enabled + default void end() {} + // called when this opmode is no longer selected; object is not reused @Override - public void disabledPeriodic() { - // this code is called periodically while the opmode is selected on the DS (robot is disabled) - } + default void close() {} - public void close() { - // this code is called when the opmode is de-selected on the DS - } - - // this function is called once to run the opmode (robot is enabled) - public abstract void run() throws InterruptedException: - - public boolean isRunning() { - // returns true until opModeStop() is called - } - - // implements OpMode interface - @Override - public final void opModeRun(long opModeId) { - run(); - } - - @Override - public final void opModeStop() { - // pseudo-code - isRunning = false; - } - - @Override - public final void opModeClose() { - close(); + // optional additional callbacks with custom timing + default Set getCallbacks() { + return Set.of(); } } ``` ```java public abstract class PeriodicOpMode implements OpMode { - // the class is constructed when the opmode is selected on the DS - - // periodic opmodes may specify their period; if unspecified, a default period of 20 ms is used protected PeriodicOpMode() {...} - protected PeriodicOpMode(double period) {...} @Override - public void disabledPeriodic() { - // this code is called periodically while the opmode is selected on the DS (robot is disabled) - } + public Set getCallbacks() {...} - public void close() { - // this code is called when the opmode is de-selected on the DS - } - - public void start() { - // this code is called when the opmode starts (robot is enabled) - } - - // this function is called periodically while the opmode is running (robot is enabled) - public abstract void periodic(); - - public void end() { - // this code is called when the opmode ends (robot is disabled) - } - - // additional periodic functions can be added using these functions + // additional periodic callbacks with custom rates/offsets public final void addPeriodic(Runnable callback, double period) {...} public final void addPeriodic(Runnable callback, double period, double offset) {...} - - // returns the start time of the current loop in microseconds - public final long getLoopStartTime() {...} - - // implements OpMode interface - @Override - public final void opModeRun(long opModeId) { - // psuedo-code - start(); - while (isRunning) { - // wait for next periodic time - // set loop start time - periodic(); // or addPeriodic() callback, as appropriate - } - end(); - } - - @Override - public final void opModeStop() { - // pseudo-code - isRunning = false; - } - - @Override - public final void opModeClose() { - close(); - } } ``` ### Annotations -All annotations are class-level. All elements are optional and may be omitted. If name is omitted, the class name is used. If group is emitted, the opmode is listed under a default group in the DS. The description is blank if it is omitted. +All annotations are class-level. All elements are optional and may be omitted. If name is omitted, the class name is used. If group is omitted, the opmode is listed as ungrouped. The description is blank if it is omitted. Color strings are parsed by `Color.fromString()`. ```java -@Autonomous(String name, String group, String description) -@Teleop(String name, String group, String description) -@TestOpMode(String name, String group, String description) +@Autonomous(String name, String group, String description, + String textColor, String backgroundColor) +@Teleop(String name, String group, String description, + String textColor, String backgroundColor) +@TestOpMode(String name, String group, String description, + String textColor, String backgroundColor) ``` Example use cases: ```java -// name will be "MyAuto", default group +// name defaults to class name; ungrouped @Autonomous -public class MyAuto extends OpModeBaseClass {...} +public class MyAuto extends PeriodicOpMode {...} -// will use default group -@Teleop("my teleop") -public class MyTeleop extends OpModeBaseClass {...} +// custom name and colors +@Teleop(name="Arcade", textColor="#FFFFFF", backgroundColor="#003366") +public class MyTeleop extends PeriodicOpMode {...} -@TestOpMode(name="my test", group="mechanisms", description="tests arm") -public class MyTest extends OpModeBaseClass {...} +@TestOpMode(name="Arm Test", group="mechanisms", description="tests arm") +public class MyTest extends PeriodicOpMode {...} ``` ### DriverStation class @@ -372,7 +336,7 @@ The following example code for non-command-based Java demonstrates the following - Robot class - A periodic autonomous opmode - A periodic teleop opmode -- A linear test opmode +- A periodic test opmode Robot: @@ -435,13 +399,26 @@ public class Teleop extends PeriodicOpMode { Test opmode: ```java -@TestOpMode("Blink dashboard indicator") -public class TestDashboardIndicator extends LinearOpMode { +@TestOpMode(name="Blink dashboard indicator") +public class TestDashboardIndicator extends PeriodicOpMode { + private final Timer timer = new Timer(); + private boolean m_logged; + @Override - public void run() { - Telemetry.log("indicator", true); - Timer.sleep(0.5); - Telemetry.log("indicator", false); + public void start() { + timer.start(); + m_logged = false; + } + + @Override + public void periodic() { + if (!m_logged) { + Telemetry.log("indicator", true); + m_logged = true; + } + if (m_logged && timer.hasElapsed(0.5)) { + Telemetry.log("indicator", false); + } } } ``` @@ -498,8 +475,6 @@ Key differences from 2025 FRC: - The per-opmode overrideable functions in Robot are replaced with separate annotated per-opmode classes (for non-command-based) - `SendableChooser` is no longer required for selecting autonomous routines; instead this functionality is integrated into opmodes, as users can create/register multiple autonomous opmodes classes, and it's been extended to support multiple teleoperated and multiple test opmodes - Selection of autonomous opmodes is integrated into the DS instead of being performed by the dashboard -- OpModes are no longer limited to just timed (periodic); a mix of different opmode functionality/approaches across different robot opmodes is possible (e.g. teleop can be periodic and autonomous linear) -- Linear opmodes are now available; these are improved versions of the old SimpleRobot/SampleRobot # Migration from 2025 FTC (FTC SDK) @@ -507,13 +482,14 @@ Key differences from 2025 FTC: - There is no hardware map. Hardware objects are instead directly instantiated inside a top-level Robot class. This Robot class is provided as a parameter to the opmode constructor. - Init is integrated into the opmode constructor. There is no equivalent to the the enabled init step; the opmode constructor is run as soon as the opmode is selected on the DS, while the robot is still disabled. Match periods will start as soon as the robot is enabled. - There is no `@Disabled` annotation for opmodes; the opmode annotation can simply be commented out to achieve the same result. +- Opmodes are all periodic. While it may be possible to create linear-like behavior, there is no equivalent of the FTC SDK's `LinearOpMode` class. # Drawbacks # Alternatives Use SendableChooser for more opmodes (teleop and test as well as autonomous); downsides of this: -- Doesn't allow flexibility of mixing linear and periodic opmodes +- Doesn't provide a first-class opmode lifecycle or opmode-specific callback scheduling model - Harder to use than annotations (generic class) - No grouping, no filtering by opmode (although these could be added) diff --git a/hal/src/main/native/cpp/jni/HALUtil.cpp b/hal/src/main/native/cpp/jni/HALUtil.cpp index 3649264a62..721b28f658 100644 --- a/hal/src/main/native/cpp/jni/HALUtil.cpp +++ b/hal/src/main/native/cpp/jni/HALUtil.cpp @@ -187,7 +187,7 @@ void ThrowBoundaryException(JNIEnv* env, double value, double lower, jobject CreateOpModeOption(JNIEnv* env, const HAL_OpModeOption& option) { static jmethodID constructor = env->GetMethodID( opModeOptionCls, "", - "(JLjava/lang/String;L/java/lang/String;Ljava/lang/String;II)V"); + "(JLjava/lang/String;Ljava/lang/String;Ljava/lang/String;II)V"); JLocal name{ env, MakeJString(env, wpi::util::to_string_view(&option.name))}; JLocal group{ diff --git a/wpilibc/robotpy_pybind_build_info.bzl b/wpilibc/robotpy_pybind_build_info.bzl index 447d1c293d..42987ff146 100644 --- a/wpilibc/robotpy_pybind_build_info.bzl +++ b/wpilibc/robotpy_pybind_build_info.bzl @@ -750,6 +750,17 @@ def wpilib_extension(srcs = [], header_to_dat_deps = [], extra_hdrs = [], includ ("wpi::internal::DriverStationModeThread", "wpi__internal__DriverStationModeThread.hpp"), ], ), + struct( + class_name = "PeriodicPriorityQueue", + yml_file = "semiwrap/PeriodicPriorityQueue.yml", + header_root = "$(execpath :robotpy-native-wpilib.copy_headers)", + header_file = "$(execpath :robotpy-native-wpilib.copy_headers)/wpi/internal/PeriodicPriorityQueue.hpp", + tmpl_class_names = [], + trampolines = [ + ("wpi::internal::PeriodicPriorityQueue", "wpi__internal__PeriodicPriorityQueue.hpp"), + ("wpi::internal::PeriodicPriorityQueue::Callback", "wpi__internal__PeriodicPriorityQueue__Callback.hpp"), + ], + ), struct( class_name = "OpMode", yml_file = "semiwrap/OpMode.yml", diff --git a/wpilibc/src/main/native/cpp/framework/OpModeRobot.cpp b/wpilibc/src/main/native/cpp/framework/OpModeRobot.cpp index 3b742ac126..05680fee47 100644 --- a/wpilibc/src/main/native/cpp/framework/OpModeRobot.cpp +++ b/wpilibc/src/main/native/cpp/framework/OpModeRobot.cpp @@ -4,11 +4,11 @@ #include "wpi/framework/OpModeRobot.hpp" -#include #include #include #include #include +#include #include @@ -17,206 +17,196 @@ #include "wpi/hal/DriverStationTypes.h" #include "wpi/hal/HAL.h" #include "wpi/hal/Notifier.hpp" +#include "wpi/hal/UsageReporting.hpp" +#include "wpi/nt/NetworkTableInstance.hpp" #include "wpi/opmode/OpMode.hpp" +#include "wpi/smartdashboard/SmartDashboard.hpp" +#include "wpi/system/Errors.hpp" +#include "wpi/system/RobotController.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 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 m_activeOpMode; -}; -} // namespace - -void OpModeRobotBase::StartCompetition() { - fmt::print("********** Robot program startup complete **********\n"); - - 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}; - +OpModeRobotBase::OpModeRobotBase(wpi::units::second_t period) + : m_period{period}, + m_loopOverrunAlert{ + fmt::format("Loop time of {:.6f}s overrun", m_period.value()), + Alert::Level::MEDIUM}, + m_watchdog{period, [this] { m_loopOverrunAlert.Set(true); }} { + // Create our own notifier and callback queue int32_t status = 0; m_notifier = HAL_CreateNotifier(&status); HAL_SetNotifierName(m_notifier, "OpModeRobot", &status); - int64_t lastModeId = -1; - bool calledObserveUserProgramStarting = false; - bool calledDriverStationConnected = false; - std::shared_ptr opMode; - WPI_EventHandle events[] = {event.GetHandle(), - static_cast(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); + m_startTime = std::chrono::microseconds{RobotController::GetMonotonicTime()}; - // Call DriverStation::ObserveUserProgramStarting() here as a one-shot to - // ensure it is called after the notifier alarm is set. The notifier alarm - // is set using relative time, so tests that wait on the user program to - // start and then step time won't work correctly if we call this before - // setting the alarm. - if (!calledObserveUserProgramStarting) { - calledObserveUserProgramStarting = true; - DriverStation::ObserveUserProgramStarting(); - } + // Add LoopFunc as periodic callback + AddPeriodic([this] { LoopFunc(); }, period); - auto signaled = wpi::util::WaitForObjects(events, signaledBuf); - if (signaled.empty()) { - return; // handles destroyed - } - for (auto signal : signaled) { - if ((signal & 0x80000000) != 0) { - return; // handle destroyed + HAL_ReportUsage("Framework", "OpModeRobot"); +} + +OpModeRobotBase::OpModeRobotBase() : OpModeRobotBase(DEFAULT_PERIOD) {} + +void OpModeRobotBase::AddPeriodic(std::function callback, + wpi::units::second_t period) { + m_callbacks.Add(std::move(callback), m_startTime, period); +} + +void OpModeRobotBase::LoopFunc() { + DriverStation::RefreshData(); + + // Get current enabled state and opmode + const hal::ControlWord word = DriverStation::GetControlWord(); + m_watchdog.Reset(); + const bool enabled = word.IsEnabled(); + int64_t modeId = word.IsDSAttached() ? word.GetOpModeId() : 0; + + if (!m_calledDriverStationConnected && word.IsDSAttached()) { + m_calledDriverStationConnected = true; + DriverStationConnected(); + m_watchdog.AddEpoch("DriverStationConnected()"); + } + + // Handle opmode changes + if (modeId != m_lastModeId) { + // Clean up current opmode + if (m_currentOpMode) { + // Remove opmode callbacks + if (m_opmodePeriodic) { + m_callbacks.Remove(*m_opmodePeriodic); + m_opmodePeriodic.reset(); } - } - - // 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; + for (auto& cb : m_activeOpModeCallbacks) { + m_callbacks.Remove(cb); } + m_activeOpModeCallbacks.clear(); + m_currentOpMode.reset(); + } + // Set up new opmode + if (modeId != 0) { auto data = m_opModes.lookup(modeId); - if (!data.factory) { + if (data.factory) { + // Instantiate the new opmode + fmt::print("********** Starting OpMode {} **********\n", data.name); + m_currentOpMode = data.factory(); + if (m_currentOpMode) { + // Ensure disabledPeriodic is called at least once + m_currentOpMode->DisabledPeriodic(); + m_watchdog.AddEpoch("OpMode::DisabledPeriodic()"); + // Register the opmode's periodic callbacks + m_opmodePeriodic = wpi::internal::PeriodicPriorityQueue::Callback{ + [op = m_currentOpMode.get()] { op->Periodic(); }, m_startTime, + m_period}; + m_callbacks.Add(*m_opmodePeriodic); + m_activeOpModeCallbacks = m_currentOpMode->GetCallbacks(); + for (auto& cb : m_activeOpModeCallbacks) { + m_callbacks.Add(cb); + } + } + } else { WPILIB_ReportError(err::Error, "No OpMode found for mode {}", modeId); - ctlWord.SetOpModeId(0); - HAL_ObserveUserProgram(ctlWord.GetValue()); - continue; } + } + m_lastModeId = modeId; + } - // 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; + // Handle enabled state changes + bool justCalledDisabledInit = false; + if (m_lastEnabledState != enabled) { + if (enabled) { + // Transitioning to enabled + DisabledExit(); + m_watchdog.AddEpoch("DisabledExit()"); + if (m_currentOpMode) { + m_currentOpMode->Start(); + m_watchdog.AddEpoch("OpMode::Start()"); } - { - std::scoped_lock lock(m_opModeMutex); - m_activeOpMode = opMode; + } else { + // Transitioning to disabled + if (m_currentOpMode && m_lastEnabledState) { + // Was enabled, now disabled + m_currentOpMode->End(); + m_watchdog.AddEpoch("OpMode::End()"); } - lastModeId = modeId; - // Ensure disabledPeriodic is called at least once - opMode->DisabledPeriodic(); + DisabledInit(); + m_watchdog.AddEpoch("DisabledInit()"); + justCalledDisabledInit = true; + } + m_lastEnabledState = enabled; + } + + // Call periodic functions based on current state + if (!enabled) { + // Only call DisabledPeriodic if we didn't just call DisabledInit + if (!justCalledDisabledInit) { + DisabledPeriodic(); + m_watchdog.AddEpoch("DisabledPeriodic()"); } - HAL_ObserveUserProgram(ctlWord.GetValue()); + // Call opmode DisabledPeriodic if we have one + if (m_currentOpMode) { + m_currentOpMode->DisabledPeriodic(); + m_watchdog.AddEpoch("OpMode::DisabledPeriodic()"); + } + } - if (ctlWord.IsEnabled()) { - // When enabled, call the opmode run function, then close and clear - wpi::util::SafeThreadOwner monitor; - monitor.Start(modeId, event, static_cast(m_notifier), - opMode); - opMode->OpModeRun(modeId); - opMode.reset(); - } else { - // When disabled, call the DisabledPeriodic function - opMode->DisabledPeriodic(); + // Call NonePeriodic when no opmode is selected + if (modeId == 0) { + NonePeriodic(); + m_watchdog.AddEpoch("NonePeriodic()"); + } + + // Always call RobotPeriodic + RobotPeriodic(); + m_watchdog.AddEpoch("RobotPeriodic()"); + + // Always observe user program state + HAL_ObserveUserProgram(word.GetValue()); + + SmartDashboard::UpdateValues(); + m_watchdog.AddEpoch("SmartDashboard::UpdateValues()"); + + if constexpr (IsSimulation()) { + HAL_SimPeriodicBefore(); + SimulationPeriodic(); + HAL_SimPeriodicAfter(); + m_watchdog.AddEpoch("SimulationPeriodic()"); + } + + m_watchdog.Disable(); + + // Flush NetworkTables + wpi::nt::NetworkTableInstance::GetDefault().FlushLocal(); + + // Warn on loop time overruns + if (m_watchdog.IsExpired()) { + m_watchdog.PrintEpochs(); + } +} + +void OpModeRobotBase::StartCompetition() { + fmt::print("********** Robot program startup complete **********\n"); + + if constexpr (IsSimulation()) { + SimulationInit(); + } + + // Tell the DS that the robot is ready to be enabled + DriverStation::ObserveUserProgramStarting(); + + // Loop forever, calling the callback system which handles periodic functions + while (true) { + if (!m_callbacks.RunCallbacks(m_notifier)) { + break; } } } void OpModeRobotBase::EndCompetition() { - m_notifier = {}; - std::shared_ptr opMode; - { - std::scoped_lock lock(m_opModeMutex); - opMode = m_activeOpMode.lock(); - } - if (opMode) { - opMode->OpModeStop(); + if (m_notifier != HAL_INVALID_HANDLE) { + HAL_DestroyNotifier(m_notifier); } } diff --git a/wpilibc/src/main/native/cpp/framework/TimedRobot.cpp b/wpilibc/src/main/native/cpp/framework/TimedRobot.cpp index 339e2484e6..14307f4136 100644 --- a/wpilibc/src/main/native/cpp/framework/TimedRobot.cpp +++ b/wpilibc/src/main/native/cpp/framework/TimedRobot.cpp @@ -10,9 +10,10 @@ #include #include "wpi/driverstation/DriverStation.hpp" -#include "wpi/hal/Notifier.hpp" +#include "wpi/hal/DriverStation.hpp" #include "wpi/hal/UsageReporting.hpp" #include "wpi/system/Errors.hpp" +#include "wpi/system/RobotController.hpp" using namespace wpi; @@ -27,45 +28,9 @@ void TimedRobot::StartCompetition() { // Loop forever, calling the appropriate mode-dependent function while (true) { - // 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) { + if (!m_callbacks.RunCallbacks(m_notifier)) { break; } - - m_loopStartTimeUs = RobotController::GetMonotonicTime(); - 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)); - } } } @@ -96,15 +61,8 @@ TimedRobot::~TimedRobot() { } } -uint64_t TimedRobot::GetLoopStartTime() { - return m_loopStartTimeUs; -} - void TimedRobot::AddPeriodic(std::function callback, wpi::units::second_t period, wpi::units::second_t offset) { - m_callbacks.emplace( - callback, m_startTime, - std::chrono::microseconds{static_cast(period.value() * 1e6)}, - std::chrono::microseconds{static_cast(offset.value() * 1e6)}); + m_callbacks.Add(std::move(callback), m_startTime, period, offset); } diff --git a/wpilibc/src/main/native/cpp/internal/PeriodicPriorityQueue.cpp b/wpilibc/src/main/native/cpp/internal/PeriodicPriorityQueue.cpp new file mode 100644 index 0000000000..55900668cf --- /dev/null +++ b/wpilibc/src/main/native/cpp/internal/PeriodicPriorityQueue.cpp @@ -0,0 +1,127 @@ +// 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/internal/PeriodicPriorityQueue.hpp" + +#include + +#include "wpi/hal/Notifier.h" +#include "wpi/system/Errors.hpp" +#include "wpi/system/RobotController.hpp" +#include "wpi/util/Synchronization.h" + +using namespace wpi::internal; + +PeriodicPriorityQueue::Callback::Callback(std::function 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{RobotController::GetMonotonicTime()} - + startTime) / + period * period) {} + +PeriodicPriorityQueue::Callback::Callback(std::function func, + std::chrono::microseconds startTime, + wpi::units::second_t period, + wpi::units::second_t offset) + : Callback{ + std::move(func), startTime, + std::chrono::microseconds{static_cast(period.value() * 1e6)}, + std::chrono::microseconds{ + static_cast(offset.value() * 1e6)}} {} + +PeriodicPriorityQueue::Callback::Callback(std::function func, + std::chrono::microseconds startTime, + wpi::units::second_t period) + : Callback{std::move(func), startTime, period, + std::chrono::microseconds{0}} {} + +void PeriodicPriorityQueue::Add(std::function func, + std::chrono::microseconds startTime, + std::chrono::microseconds period) { + Add(std::move(func), startTime, period, std::chrono::microseconds{0}); +} + +void PeriodicPriorityQueue::Add(std::function func, + std::chrono::microseconds startTime, + std::chrono::microseconds period, + std::chrono::microseconds offset) { + m_queue.emplace(std::move(func), startTime, period, offset); +} + +void PeriodicPriorityQueue::Add(std::function func, + std::chrono::microseconds startTime, + wpi::units::second_t period) { + Add(std::move(func), startTime, period, wpi::units::second_t{0}); +} + +void PeriodicPriorityQueue::Add(std::function func, + std::chrono::microseconds startTime, + wpi::units::second_t period, + wpi::units::second_t offset) { + m_queue.emplace(std::move(func), startTime, period, offset); +} + +void PeriodicPriorityQueue::Add(Callback callback) { + m_queue.push(std::move(callback)); +} + +bool PeriodicPriorityQueue::Remove(const Callback& callback) { + return m_queue.remove(callback); +} + +void PeriodicPriorityQueue::Clear() { + while (!m_queue.empty()) { + m_queue.pop(); + } +} + +bool PeriodicPriorityQueue::RunCallbacks(HAL_NotifierHandle notifier) { + // 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_queue.pop(); + + int32_t status = 0; + HAL_SetNotifierAlarm(notifier, callback.expirationTime.count(), 0, true, true, + &status); + WPILIB_CheckErrorStatus(status, "SetNotifierAlarm"); + + if (WPI_WaitForObject(notifier) == 0) { + return false; + } + + const std::chrono::microseconds currentTime{ + RobotController::GetMonotonicTime()}; + m_loopStartTime = wpi::units::microsecond_t{currentTime}; + + 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_queue.push(std::move(callback)); + + // Process all other callbacks that are ready to run + while (m_queue.top().expirationTime <= currentTime) { + callback = m_queue.pop(); + + callback.func(); + + callback.expirationTime += + callback.period + (currentTime - callback.expirationTime) / + callback.period * callback.period; + m_queue.push(std::move(callback)); + } + + return true; +} diff --git a/wpilibc/src/main/native/cpp/opmode/PeriodicOpMode.cpp b/wpilibc/src/main/native/cpp/opmode/PeriodicOpMode.cpp index c14fd39836..afee5eefe6 100644 --- a/wpilibc/src/main/native/cpp/opmode/PeriodicOpMode.cpp +++ b/wpilibc/src/main/native/cpp/opmode/PeriodicOpMode.cpp @@ -6,154 +6,22 @@ #include -#include "wpi/driverstation/DriverStation.hpp" -#include "wpi/hal/DriverStation.h" -#include "wpi/hal/Notifier.hpp" #include "wpi/hal/UsageReporting.hpp" -#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 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::GetMonotonicTime()} - - startTime) / - period * period) {} - -PeriodicOpMode::~PeriodicOpMode() { - if (m_notifier != HAL_INVALID_HANDLE) { - 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::GetMonotonicTime()}; - AddPeriodic([=, this] { LoopFunc(); }, period); - - int32_t status = 0; - m_notifier = HAL_CreateNotifier(&status); - WPILIB_CheckErrorStatus(status, "CreateNotifier"); - HAL_SetNotifierName(m_notifier, "PeriodicOpMode", &status); - +PeriodicOpMode::PeriodicOpMode() + : m_startTime{ + std::chrono::microseconds{RobotController::GetMonotonicTime()}} { HAL_ReportUsage("OpMode", "PeriodicOpMode"); } void PeriodicOpMode::AddPeriodic(std::function callback, wpi::units::second_t period, wpi::units::second_t offset) { - m_callbacks.emplace( - callback, m_startTime, + m_callbacks.emplace_back( + std::move(callback), m_startTime, std::chrono::microseconds{static_cast(period.value() * 1e6)}, std::chrono::microseconds{static_cast(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::GetMonotonicTime(); - 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_INVALID_HANDLE; -} - -void PeriodicOpMode::PrintLoopOverrunMessage() { - WPILIB_ReportWarning("Loop time of {:.6f}s overrun", m_period.value()); -} - -void PeriodicOpMode::PrintWatchdogEpochs() { - m_watchdog.PrintEpochs(); -} diff --git a/wpilibc/src/main/native/include/wpi/framework/OpModeRobot.hpp b/wpilibc/src/main/native/include/wpi/framework/OpModeRobot.hpp index f7ac88d900..c817cd8d76 100644 --- a/wpilibc/src/main/native/include/wpi/framework/OpModeRobot.hpp +++ b/wpilibc/src/main/native/include/wpi/framework/OpModeRobot.hpp @@ -5,17 +5,22 @@ #pragma once #include -#include #include #include #include +#include +#include "wpi/driverstation/Alert.hpp" #include "wpi/framework/RobotBase.hpp" #include "wpi/hal/DriverStationTypes.hpp" #include "wpi/hal/Notifier.h" -#include "wpi/hal/Types.hpp" +#include "wpi/internal/PeriodicPriorityQueue.hpp" #include "wpi/opmode/OpMode.hpp" +#include "wpi/system/Watchdog.hpp" +#include "wpi/units/time.hpp" #include "wpi/util/DenseMap.hpp" +#include "wpi/util/SafeThread.hpp" +#include "wpi/util/Synchronization.hpp" #include "wpi/util/mutex.hpp" namespace wpi::util { @@ -37,7 +42,7 @@ concept OneArgOpMode = std::constructible_from && OpModeDerived; /** * Concept indicating a class is derived from OpMode and has either a - * no-argument constructor or a constructorthat accepts R&. + * no-argument constructor or a constructor that accepts R&. * * @tparam T opmode class * @tparam R robot class @@ -60,6 +65,9 @@ class OpModeRobotBase : public RobotBase { public: using OpModeFactory = std::function()>; + /// Default loop period. + static constexpr auto DEFAULT_PERIOD = 20_ms; + /** * Provide an alternate "main loop" via StartCompetition(). */ @@ -72,14 +80,20 @@ class OpModeRobotBase : public RobotBase { /** * Constructor. + * + * @param period The period of the robot loop function. */ - OpModeRobotBase() = default; + explicit OpModeRobotBase(wpi::units::second_t period); + + /** + * Constructor for an OpModeRobot with a default loop time of 0.02 seconds. + */ + explicit OpModeRobotBase(); + 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 @@ -87,12 +101,65 @@ class OpModeRobotBase : public RobotBase { */ virtual void DriverStationConnected() {} + /** + * Function called periodically every loop, regardless of enabled state or + * OpMode selection. + */ + virtual void RobotPeriodic() {} + + /** + * Function called once during robot initialization in simulation. + */ + virtual void SimulationInit() {} + + /** + * Function called periodically in simulation. + */ + virtual void SimulationPeriodic() {} + + /** + * Function called once when the robot becomes disabled. + */ + virtual void DisabledInit() {} + + /** + * Function called periodically while the robot is disabled. + */ + virtual void DisabledPeriodic() {} + + /** + * Function called once when the robot exits disabled state. + */ + virtual void DisabledExit() {} + /** * Function called periodically anytime when no opmode is selected, including * when the Driver Station is disconnected. */ virtual void NonePeriodic() {} + /** + * Add a callback to run at a specific period. + * + * @param callback The callback to run. + * @param period The period at which to run the callback. + */ + void AddPeriodic(std::function callback, wpi::units::second_t period); + + /** + * Return the system clock time in microseconds for the start of the current + * periodic loop. This is in the same time base as + * Timer.getMonotonicTimeStamp(), 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. + */ + wpi::units::microsecond_t GetLoopStartTime() const { + return m_callbacks.GetLoopStartTime(); + } + /** * Adds an operating mode option using a factory function that creates the * opmode. It's necessary to call PublishOpModes() to make the added modes @@ -147,15 +214,37 @@ class OpModeRobotBase : public RobotBase { */ void ClearOpModes(); + protected: + /** + * Main robot loop function. Handles disabled state logic. + */ + void LoopFunc(); + private: struct OpModeData { std::string name; OpModeFactory factory; }; + wpi::util::DenseMap m_opModes; - wpi::hal::Handle m_notifier; wpi::util::mutex m_opModeMutex; - std::weak_ptr m_activeOpMode; + + wpi::internal::PeriodicPriorityQueue m_callbacks; + HAL_NotifierHandle m_notifier; + wpi::units::second_t m_period; + std::chrono::microseconds m_startTime; + Alert m_loopOverrunAlert; + Watchdog m_watchdog; + + // OpMode lifecycle state + int64_t m_lastModeId = -1; + bool m_calledDriverStationConnected = false; + bool m_lastEnabledState = false; + std::shared_ptr m_currentOpMode; + std::vector + m_activeOpModeCallbacks; + std::optional + m_opmodePeriodic; }; /** @@ -176,6 +265,18 @@ class OpModeRobotBase : public RobotBase { template class OpModeRobot : public OpModeRobotBase { public: + /** + * Constructor. + * + * @param period The period of the robot loop function. + */ + explicit OpModeRobot(wpi::units::second_t period) : OpModeRobotBase{period} {} + + /** + * Constructor for an OpModeRobot with a default loop time of 0.02 seconds. + */ + explicit OpModeRobot() : OpModeRobotBase{} {} + /** * Adds an operating mode option. It's necessary to call PublishOpModes() to * make the added modes visible to the driver station. diff --git a/wpilibc/src/main/native/include/wpi/framework/TimedRobot.hpp b/wpilibc/src/main/native/include/wpi/framework/TimedRobot.hpp index bba8f07a98..66c7d32520 100644 --- a/wpilibc/src/main/native/include/wpi/framework/TimedRobot.hpp +++ b/wpilibc/src/main/native/include/wpi/framework/TimedRobot.hpp @@ -4,18 +4,14 @@ #pragma once -#include #include -#include -#include #include "wpi/framework/IterativeRobotBase.hpp" -#include "wpi/hal/Notifier.h" +#include "wpi/hal/Notifier.hpp" #include "wpi/hal/Types.hpp" -#include "wpi/system/RobotController.hpp" +#include "wpi/internal/PeriodicPriorityQueue.hpp" #include "wpi/units/frequency.hpp" #include "wpi/units/time.hpp" -#include "wpi/util/priority_queue.hpp" namespace wpi { @@ -64,7 +60,7 @@ class TimedRobot : public IterativeRobotBase { /** * Return the system clock time in microseconds for the start of the current - * periodic loop. This is in the same time base as + * periodic loop. This is in the same time base as * Timer.GetMonotonicTimestamp(), but is stable through a loop. It is updated * at the beginning of every periodic callback (including the normal periodic * loop). @@ -72,7 +68,9 @@ class TimedRobot : public IterativeRobotBase { * @return Robot running time in microseconds, as of the start of the current * periodic function. */ - uint64_t GetLoopStartTime(); + wpi::units::microsecond_t GetLoopStartTime() const { + return m_callbacks.GetLoopStartTime(); + } /** * Add a callback to run at a specific period with a starting time offset. @@ -89,43 +87,12 @@ class TimedRobot : public IterativeRobotBase { void AddPeriodic(std::function callback, wpi::units::second_t period, wpi::units::second_t offset = 0_s); - private: - class Callback { - public: - std::function 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 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::GetMonotonicTime()} - - startTime) / - period * period) {} - - bool operator>(const Callback& rhs) const { - return expirationTime > rhs.expirationTime; - } - }; - + protected: wpi::hal::Handle m_notifier; std::chrono::microseconds m_startTime; - uint64_t m_loopStartTimeUs = 0; - wpi::util::priority_queue, - std::greater> - m_callbacks; + private: + wpi::internal::PeriodicPriorityQueue m_callbacks; }; } // namespace wpi diff --git a/wpilibc/src/main/native/include/wpi/internal/PeriodicPriorityQueue.hpp b/wpilibc/src/main/native/include/wpi/internal/PeriodicPriorityQueue.hpp new file mode 100644 index 0000000000..3f6c6e6eea --- /dev/null +++ b/wpilibc/src/main/native/include/wpi/internal/PeriodicPriorityQueue.hpp @@ -0,0 +1,206 @@ +// 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 +#include +#include + +#include "wpi/hal/Notifier.h" +#include "wpi/units/time.hpp" +#include "wpi/util/priority_queue.hpp" + +namespace wpi::internal { + +/** + * A priority queue for scheduling periodic callbacks based on their next + * execution time. + * + *

This class manages a collection of periodic callbacks that execute at + * specified intervals. Callbacks are scheduled using FPGA timestamps and + * automatically rescheduled after execution to maintain their periodic + * behavior. The queue uses a priority heap to efficiently determine the next + * callback to execute. + * + *

This is an internal scheduling primitive used by robot frameworks like + * TimedRobot. + */ +class PeriodicPriorityQueue { + public: + /** + * A periodic callback with scheduling metadata. + * + * Each callback tracks its target function, period, and next expiration + * time. After execution, the expiration time is automatically advanced by + * full periods to maintain precise timing even when execution is delayed. + */ + class Callback { + public: + /** The function to execute when the callback fires. */ + std::function func; + + /** The period at which to run the callback. */ + std::chrono::microseconds period; + + /** The next scheduled execution time in FPGA timestamp microseconds. */ + 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 func, std::chrono::microseconds startTime, + std::chrono::microseconds period, + std::chrono::microseconds offset); + + /** + * Construct a callback container using units-based period and offset. + * + * @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 func, std::chrono::microseconds startTime, + wpi::units::second_t period, wpi::units::second_t offset); + + /** + * Construct a callback container using units-based period. + * + * @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. + */ + Callback(std::function func, std::chrono::microseconds startTime, + wpi::units::second_t period); + + bool operator>(const Callback& rhs) const { + return expirationTime > rhs.expirationTime; + } + + bool operator==(const Callback& rhs) const { + return expirationTime == rhs.expirationTime; + } + }; + + /** + * Adds a periodic callback to the queue. + * + * @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. + */ + void Add(std::function func, std::chrono::microseconds startTime, + std::chrono::microseconds period); + + /** + * Adds a periodic callback to the queue. + * + * @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. + */ + void Add(std::function func, std::chrono::microseconds startTime, + std::chrono::microseconds period, std::chrono::microseconds offset); + + /** + * Adds a periodic callback to the queue. + * + * @param func The callback to run. + * @param startTime The common starting point for all callback scheduling in + * FPGA timestamp microseconds. + * @param period The period at which to run the callback. + */ + void Add(std::function func, std::chrono::microseconds startTime, + wpi::units::second_t period); + + /** + * Adds a periodic callback to the queue. + * + * @param func The callback to run. + * @param startTime The common starting point for all callback scheduling in + * FPGA timestamp microseconds. + * @param period The period at which to run the callback. + * @param offset The offset from the common starting time. + */ + void Add(std::function func, std::chrono::microseconds startTime, + wpi::units::second_t period, wpi::units::second_t offset); + + /** + * Adds a pre-constructed callback to the queue. + * + * @param callback The callback to add. + */ + void Add(Callback callback); + + /** + * Removes a specific callback from the queue. + * + * @param callback The callback to remove. + * @return true if the callback was found and removed, false otherwise. + */ + bool Remove(const Callback& callback); + + /** + * Removes all callbacks from the queue. + */ + void Clear(); + + /** + * Executes all callbacks that are due, then waits for the next callback's + * scheduled time. + * + * This method performs the following steps: + * -# Retrieves the callback with the earliest expiration time from the queue + * -# Sets a hardware notifier alarm to wait until that callback's expiration + * time + * -# Blocks until the notifier signals or is interrupted + * -# Executes the callback and reschedules it for its next period + * -# Processes any additional callbacks that have become due during execution + * + * When rescheduling callbacks, this method automatically compensates for + * execution delays by advancing the expiration time by the number of full + * periods that have elapsed, ensuring callbacks maintain their scheduled + * phase over time. + * + * @param notifier The HAL notifier handle to use for timing. + * @return false if the notifier was destroyed (loop should exit), true + * otherwise. + */ + bool RunCallbacks(HAL_NotifierHandle notifier); + + /** + * Returns the underlying priority queue. + */ + wpi::util::priority_queue, std::greater<>>& + GetQueue() { + return m_queue; + } + + /** + * Return the system clock time in microseconds for the start of the current + * periodic loop. This is in the same time base as + * Timer.getMonotonicTimeStamp(), 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. + */ + wpi::units::microsecond_t GetLoopStartTime() const { return m_loopStartTime; } + + private: + wpi::util::priority_queue, std::greater<>> + m_queue; + + wpi::units::microsecond_t m_loopStartTime{0}; +}; + +} // namespace wpi::internal diff --git a/wpilibc/src/main/native/include/wpi/opmode/OpMode.hpp b/wpilibc/src/main/native/include/wpi/opmode/OpMode.hpp index dec1a8b064..48f1d8716e 100644 --- a/wpilibc/src/main/native/include/wpi/opmode/OpMode.hpp +++ b/wpilibc/src/main/native/include/wpi/opmode/OpMode.hpp @@ -4,7 +4,9 @@ #pragma once -#include +#include + +#include "wpi/internal/PeriodicPriorityQueue.hpp" namespace wpi { @@ -12,34 +14,69 @@ namespace wpi { * Top-level interface for opmode classes. Users should generally extend one of * the abstract implementations of this interface (e.g. PeriodicOpMode) rather * than directly implementing this interface. + * + *

Lifecycle: + *

    + *
  • constructed when opmode selected on driver station + *
  • DisabledPeriodic() called periodically as long as DS is disabled + *
  • when DS transitions from disabled to enabled, Start() is called once + *
  • while DS is enabled, Periodic() is called periodically and additional + * periodic callbacks added via GetCallbacks() are called periodically + *
  • when DS transitions from enabled to disabled, or a different opmode is + * selected while enabled, End() is called, followed by Close(); the + * object is not reused + *
  • if a different opmode is selected while disabled, only Close() is + * called; the object is not reused + *
*/ class OpMode { public: /** * The object is destroyed when the opmode is no longer selected on the DS or - * after OpModeRun() returns. + * after an enabled run ends. The object will not be reused after the + * destructor is called. */ 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 + * 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; + /** Called once when this opmode transitions to enabled. */ + virtual void Start() {} /** - * This function is called asynchronously when the robot is disabled, to - * request the opmode return from OpModeRun(). + * This function is called periodically while the opmode is enabled. */ - virtual void OpModeStop() = 0; + virtual void Periodic() {} + + /** + * This function is called when the robot disables or switches opmodes while + * this opmode is enabled. Implementations should stop blocking work + * promptly. + */ + virtual void End() {} + + /** + * Returns a vector of custom periodic callbacks to be executed while the + * opmode is enabled. + * + *

This method allows opmodes to register arbitrary periodic callbacks + * with custom execution intervals. The callbacks are executed by the robot + * framework at their scheduled times, in addition to the primary Periodic() + * callback. + * + * @return A vector of custom callbacks to execute, or an empty vector if no + * custom callbacks are needed. The default implementation returns an + * empty vector. + */ + virtual std::vector + GetCallbacks() { + return {}; + } }; } // namespace wpi diff --git a/wpilibc/src/main/native/include/wpi/opmode/PeriodicOpMode.hpp b/wpilibc/src/main/native/include/wpi/opmode/PeriodicOpMode.hpp index 9ae472fccb..1c5892ec6f 100644 --- a/wpilibc/src/main/native/include/wpi/opmode/PeriodicOpMode.hpp +++ b/wpilibc/src/main/native/include/wpi/opmode/PeriodicOpMode.hpp @@ -4,29 +4,24 @@ #pragma once -#include - #include #include +#include #include -#include "wpi/hal/Notifier.h" -#include "wpi/hal/Types.hpp" +#include "wpi/internal/PeriodicPriorityQueue.hpp" #include "wpi/opmode/OpMode.hpp" -#include "wpi/system/Watchdog.hpp" +#include "wpi/system/RobotController.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. + * period). The primary periodic callback function is the abstract Periodic() + * function. Additional periodic callbacks with different intervals can be added + * using the AddPeriodic() set of functions. * * Lifecycle: * @@ -39,33 +34,25 @@ namespace wpi { * - 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 + * set by the robot framework, 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 + * followed by Close(); 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 + * disabled, only Close() is called; the object is not reused */ class PeriodicOpMode : public OpMode { - public: - /** Default loop period. */ - static constexpr auto DEFAULT_PERIOD = 20_ms; - protected: /** - * Constructor. Periodic opmodes may specify the period used for the - * Periodic() function. - * - * @param period period for callbacks to the Periodic() function + * Constructor. */ - explicit PeriodicOpMode(wpi::units::second_t period = DEFAULT_PERIOD); + PeriodicOpMode(); public: - ~PeriodicOpMode() override; + ~PeriodicOpMode() override = default; /** * Called periodically while the opmode is selected on the DS (robot is @@ -77,35 +64,52 @@ class PeriodicOpMode : public OpMode { * Called a single time when the robot transitions from disabled to enabled. * This is called prior to Periodic() being called. */ - virtual void Start() {} + void Start() override {} - /** Called periodically while the robot is enabled. */ - virtual void Periodic() = 0; + /** + * Called periodically while the robot is enabled. The framework calls this + * at OpModeRobot's configured loop period. + */ + void Periodic() override {} /** * 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. + * or just before Close() is called if a different opmode is selected while + * the robot is enabled. */ - virtual void End() {} + void End() override {} /** - * Return the system clock time in microseconds for the start of the current - * periodic loop. This is in the same time base as - * Timer::GetMonotonicTimestamp(), but is stable through a loop. It is updated - * at the beginning of every periodic callback (including the normal periodic - * loop). + * Returns the set of additional periodic callbacks registered via + * AddPeriodic(). These are executed by the robot framework while the opmode + * is enabled, in addition to the primary Periodic() callback. * - * @return Robot running time in microseconds, as of the start of the current - * periodic function. + * @return The vector of additional periodic callbacks. */ - int64_t GetLoopStartTime() const { return m_loopStartTimeUs; } + std::vector GetCallbacks() + override { + return m_callbacks; + } + + /** + * Add a callback to run at a specific period. + * + * 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. + */ + void AddPeriodic(std::function callback, + wpi::units::second_t period) { + AddPeriodic(std::move(callback), period, period); + } /** * 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. + * 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. @@ -114,66 +118,11 @@ class PeriodicOpMode : public OpMode { * to TimedRobot. */ void AddPeriodic(std::function 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; + wpi::units::second_t offset); private: - class Callback { - public: - std::function 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 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 m_notifier; + std::vector m_callbacks; std::chrono::microseconds m_startTime; - int64_t m_loopStartTimeUs = 0; - wpi::units::second_t m_period; - Watchdog m_watchdog; - - wpi::util::priority_queue, - std::greater> - m_callbacks; - - void PrintLoopOverrunMessage(); }; } // namespace wpi diff --git a/wpilibc/src/main/python/pyproject.toml b/wpilibc/src/main/python/pyproject.toml index 2df9f6e608..c25ac12bf3 100644 --- a/wpilibc/src/main/python/pyproject.toml +++ b/wpilibc/src/main/python/pyproject.toml @@ -197,6 +197,7 @@ Encoder = "wpi/hardware/rotation/Encoder.hpp" # wpi/internal DriverStationModeThread = "wpi/internal/DriverStationModeThread.hpp" +PeriodicPriorityQueue = "wpi/internal/PeriodicPriorityQueue.hpp" # wpi/opmode OpMode = "wpi/opmode/OpMode.hpp" diff --git a/wpilibc/src/main/python/semiwrap/OpMode.yml b/wpilibc/src/main/python/semiwrap/OpMode.yml index 6a06592005..e4c4179d28 100644 --- a/wpilibc/src/main/python/semiwrap/OpMode.yml +++ b/wpilibc/src/main/python/semiwrap/OpMode.yml @@ -2,5 +2,7 @@ classes: wpi::OpMode: methods: DisabledPeriodic: - OpModeRun: - OpModeStop: + Start: + Periodic: + End: + GetCallbacks: diff --git a/wpilibc/src/main/python/semiwrap/OpModeRobot.yml b/wpilibc/src/main/python/semiwrap/OpModeRobot.yml index 9222607e32..20f7f43d58 100644 --- a/wpilibc/src/main/python/semiwrap/OpModeRobot.yml +++ b/wpilibc/src/main/python/semiwrap/OpModeRobot.yml @@ -5,8 +5,10 @@ classes: wpi::OpModeRobotBase: methods: StartCompetition: - EndCompetition: OpModeRobotBase: + overloads: + wpi::units::second_t: + "": DriverStationConnected: NonePeriodic: AddOpModeFactory: @@ -17,5 +19,26 @@ classes: RemoveOpMode: PublishOpModes: ClearOpModes: + EndCompetition: + RobotPeriodic: + SimulationInit: + SimulationPeriodic: + DisabledInit: + DisabledPeriodic: + DisabledExit: + AddPeriodic: + GetLoopStartTime: + LoopFunc: + attributes: + DEFAULT_PERIOD: wpi::OpModeRobot: ignore: true + methods: + OpModeRobot: + overloads: + wpi::units::second_t: + "": + 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: diff --git a/wpilibc/src/main/python/semiwrap/PeriodicOpMode.yml b/wpilibc/src/main/python/semiwrap/PeriodicOpMode.yml index fdc34324c6..3ba7c756fd 100644 --- a/wpilibc/src/main/python/semiwrap/PeriodicOpMode.yml +++ b/wpilibc/src/main/python/semiwrap/PeriodicOpMode.yml @@ -1,17 +1,13 @@ classes: wpi::PeriodicOpMode: - attributes: - DEFAULT_PERIOD: methods: DisabledPeriodic: Start: Periodic: End: - GetLoopStartTime: AddPeriodic: - GetPeriod: - PrintWatchdogEpochs: - OpModeRun: - OpModeStop: + overloads: + std::function, wpi::units::second_t: + std::function, wpi::units::second_t, wpi::units::second_t: PeriodicOpMode: - LoopFunc: + GetCallbacks: diff --git a/wpilibc/src/main/python/semiwrap/PeriodicPriorityQueue.yml b/wpilibc/src/main/python/semiwrap/PeriodicPriorityQueue.yml new file mode 100644 index 0000000000..5b0aff0889 --- /dev/null +++ b/wpilibc/src/main/python/semiwrap/PeriodicPriorityQueue.yml @@ -0,0 +1,28 @@ +classes: + wpi::internal::PeriodicPriorityQueue: + methods: + Add: + overloads: + std::function, std::chrono::microseconds, std::chrono::microseconds: + std::function, std::chrono::microseconds, std::chrono::microseconds, std::chrono::microseconds: + std::function, std::chrono::microseconds, wpi::units::second_t: + std::function, std::chrono::microseconds, wpi::units::second_t, wpi::units::second_t: + Callback: + Remove: + Clear: + RunCallbacks: + GetQueue: + GetLoopStartTime: + wpi::internal::PeriodicPriorityQueue::Callback: + attributes: + func: + period: + expirationTime: + methods: + Callback: + overloads: + std::function, std::chrono::microseconds, std::chrono::microseconds, std::chrono::microseconds: + std::function, std::chrono::microseconds, wpi::units::second_t, wpi::units::second_t: + std::function, std::chrono::microseconds, wpi::units::second_t: + operator>: + operator==: diff --git a/wpilibc/src/main/python/semiwrap/TimedRobot.yml b/wpilibc/src/main/python/semiwrap/TimedRobot.yml index 469f818d24..3fe79f9c43 100644 --- a/wpilibc/src/main/python/semiwrap/TimedRobot.yml +++ b/wpilibc/src/main/python/semiwrap/TimedRobot.yml @@ -2,6 +2,8 @@ classes: wpi::TimedRobot: attributes: DEFAULT_PERIOD: + m_notifier: + m_startTime: methods: StartCompetition: EndCompetition: diff --git a/wpilibc/src/test/native/cpp/OpModeRobotTest.cpp b/wpilibc/src/test/native/cpp/OpModeRobotTest.cpp index 1853a27dfe..72a4f7e05b 100644 --- a/wpilibc/src/test/native/cpp/OpModeRobotTest.cpp +++ b/wpilibc/src/test/native/cpp/OpModeRobotTest.cpp @@ -13,6 +13,8 @@ #include "wpi/util/Color.hpp" #include "wpi/util/string.hpp" +inline constexpr auto kPeriod = 20_ms; + namespace { class OpModeRobotTest : public ::testing::Test { protected: @@ -29,20 +31,24 @@ class MockRobot; class MockOpMode : public wpi::OpMode { public: std::atomic m_disabledPeriodicCount{0}; - std::atomic m_opModeRunCount{0}; - std::atomic m_opModeStopCount{0}; + std::atomic m_startCount{0}; + std::atomic m_periodicCount{0}; + std::atomic m_endCount{0}; + std::atomic m_closeCount{0}; MockOpMode() = default; + ~MockOpMode() override { m_closeCount++; } void DisabledPeriodic() override { m_disabledPeriodicCount++; } - void OpModeRun(int64_t opModeId) override { m_opModeRunCount++; } - void OpModeStop() override { m_opModeStopCount++; } + void Start() override { m_startCount++; } + void Periodic() override { m_periodicCount++; } + void End() override { m_endCount++; } }; class OneArgOpMode : public wpi::OpMode { public: explicit OneArgOpMode(MockRobot& robot) {} - void OpModeRun(int64_t opModeId) override {} - void OpModeStop() override {} + void Start() override {} + void End() override {} }; class MockRobot : public wpi::OpModeRobot { @@ -50,11 +56,16 @@ class MockRobot : public wpi::OpModeRobot { std::atomic m_driverStationConnectedCount{0}; std::atomic m_nonePeriodicCount{0}; + // RobotPeriodic method counter + std::atomic m_robotPeriodicCount{0}; + MockRobot() = default; void DriverStationConnected() override { m_driverStationConnectedCount++; } void NonePeriodic() override { m_nonePeriodicCount++; } + + void RobotPeriodic() override { m_robotPeriodicCount++; } }; } // namespace @@ -167,9 +178,36 @@ TEST_F(OpModeRobotTest, NonePeriodic) { std::thread robotThread{[&] { robot.StartCompetition(); }}; wpi::sim::WaitForProgramStart(); - // Time step to get periodic calls on 50 ms timeout + // Time step to get periodic calls on 20 ms robot loop wpi::sim::StepTiming(110_ms); - EXPECT_EQ(robot.m_nonePeriodicCount.load(), 2u); + EXPECT_EQ(robot.m_nonePeriodicCount.load(), 5u); + + robot.EndCompetition(); + robotThread.join(); +} + +TEST_F(OpModeRobotTest, RobotPeriodic) { + struct MyMockRobot : public MockRobot { + MyMockRobot() { + AddOpMode(wpi::RobotMode::TELEOPERATED, "TestOpMode"); + PublishOpModes(); + } + }; + MyMockRobot robot; + + std::thread robotThread{[&] { robot.StartCompetition(); }}; + wpi::sim::WaitForProgramStart(); + + // RobotPeriodic should be called regardless of state + EXPECT_EQ(robot.m_robotPeriodicCount.load(), 0u); + + // Step timing to allow callbacks to execute + wpi::sim::StepTiming(kPeriod); + EXPECT_EQ(robot.m_robotPeriodicCount.load(), 1u); + + // Additional time steps should continue calling RobotPeriodic + wpi::sim::StepTiming(kPeriod); + EXPECT_EQ(robot.m_robotPeriodicCount.load(), 2u); robot.EndCompetition(); robotThread.join(); diff --git a/wpilibc/src/test/python/test_opmode_robot.py b/wpilibc/src/test/python/test_opmode_robot.py index 413a85746d..088c0ff7b7 100644 --- a/wpilibc/src/test/python/test_opmode_robot.py +++ b/wpilibc/src/test/python/test_opmode_robot.py @@ -146,6 +146,7 @@ def test_none_periodic(): robot = MyMockRobot() robot_thread = threading.Thread(target=robot.startCompetition) + robot_thread.daemon = True # Make thread daemon so it doesn't block test exit robot_thread.start() wsim.waitForProgramStart() @@ -155,4 +156,4 @@ def test_none_periodic(): assert robot.none_periodic_count == 2 robot.endCompetition() - robot_thread.join() + robot_thread.join(timeout=1.0) # Add timeout to prevent hanging diff --git a/wpilibcExamples/src/main/cpp/snippets/FlywheelBangBangController/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/snippets/FlywheelBangBangController/cpp/Robot.cpp index 2dbe0edba9..d3fb7ca084 100644 --- a/wpilibcExamples/src/main/cpp/snippets/FlywheelBangBangController/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/snippets/FlywheelBangBangController/cpp/Robot.cpp @@ -12,6 +12,7 @@ #include "wpi/simulation/EncoderSim.hpp" #include "wpi/simulation/FlywheelSim.hpp" #include "wpi/smartdashboard/SmartDashboard.hpp" +#include "wpi/system/RobotController.hpp" #include "wpi/units/moment_of_inertia.hpp" /** diff --git a/wpilibj/src/main/java/org/wpilib/framework/IterativeRobotBase.java b/wpilibj/src/main/java/org/wpilib/framework/IterativeRobotBase.java index a44d876942..baa63126c6 100644 --- a/wpilibj/src/main/java/org/wpilib/framework/IterativeRobotBase.java +++ b/wpilibj/src/main/java/org/wpilib/framework/IterativeRobotBase.java @@ -235,7 +235,7 @@ public abstract class IterativeRobotBase extends RobotBase { } /** Loop function. */ - protected void loopFunc() { + protected final void loopFunc() { DriverStation.refreshData(); DriverStation.refreshControlWordFromCache(m_word); m_watchdog.reset(); diff --git a/wpilibj/src/main/java/org/wpilib/framework/OpModeRobot.java b/wpilibj/src/main/java/org/wpilib/framework/OpModeRobot.java index 804c88cbff..7869f66079 100644 --- a/wpilibj/src/main/java/org/wpilib/framework/OpModeRobot.java +++ b/wpilibj/src/main/java/org/wpilib/framework/OpModeRobot.java @@ -4,18 +4,22 @@ package org.wpilib.framework; +import static org.wpilib.units.Units.Seconds; + import java.io.File; import java.io.IOException; import java.lang.reflect.Modifier; import java.net.URL; import java.util.Enumeration; import java.util.HashMap; +import java.util.HashSet; import java.util.Map; import java.util.Optional; -import java.util.concurrent.atomic.AtomicReference; +import java.util.Set; import java.util.function.Supplier; import java.util.jar.JarEntry; import java.util.jar.JarFile; +import org.wpilib.driverstation.Alert; import org.wpilib.driverstation.DriverStation; import org.wpilib.driverstation.UserControls; import org.wpilib.driverstation.UserControlsInstance; @@ -24,13 +28,19 @@ import org.wpilib.hardware.hal.DriverStationJNI; import org.wpilib.hardware.hal.HAL; import org.wpilib.hardware.hal.NotifierJNI; import org.wpilib.hardware.hal.RobotMode; +import org.wpilib.internal.PeriodicPriorityQueue; +import org.wpilib.internal.PeriodicPriorityQueue.Callback; +import org.wpilib.networktables.NetworkTableInstance; import org.wpilib.opmode.Autonomous; import org.wpilib.opmode.OpMode; +import org.wpilib.opmode.PeriodicOpMode; import org.wpilib.opmode.Teleop; import org.wpilib.opmode.TestOpMode; +import org.wpilib.smartdashboard.SmartDashboard; +import org.wpilib.system.RobotController; +import org.wpilib.system.Watchdog; import org.wpilib.util.Color; import org.wpilib.util.ConstructorMatch; -import org.wpilib.util.WPIUtilJNI; /** * OpModeRobot implements the opmode-based robot program framework. @@ -38,13 +48,15 @@ import org.wpilib.util.WPIUtilJNI; *

The OpModeRobot class is intended to be subclassed by a user creating a robot program. * *

Classes annotated with {@link Autonomous}, {@link Teleop}, and {@link TestOpMode} in the same - * package or subpackages as the user's subclass will be automatically registered as autonomous, - * teleop, and test opmodes respectively. + * package or subpackages as the user's subclass are automatically registered as autonomous, teleop, + * and test opmodes respectively. * - *

Opmodes are constructed when selected on the driver station, and closed/no longer used 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. + *

Opmodes are constructed when selected on the driver station. While selected and disabled, + * {@link PeriodicOpMode#disabledPeriodic()} is called. When enabled, {@link PeriodicOpMode#start()} + * is called once and {@link PeriodicOpMode#periodic()} runs at the rate from {@link #getPeriod()}. + * On disable or mode switch while enabled, {@link PeriodicOpMode#end()} is called asynchronously + * and the opmode is then closed and discarded. When no opmode is selected, {@link #nonePeriodic()} + * is called. {@link #driverStationConnected()} is called once when the DS first connects. */ public abstract class OpModeRobot extends RobotBase { private final ControlWord m_word = new ControlWord(); @@ -52,8 +64,22 @@ public abstract class OpModeRobot extends RobotBase { private record OpModeFactory(String name, Supplier supplier) {} private final Map m_opModes = new HashMap<>(); - private final AtomicReference m_activeOpMode = new AtomicReference<>(null); - private volatile int m_notifier; + + // Callback system fields (match C++ architecture) + private final PeriodicPriorityQueue m_callbacks = new PeriodicPriorityQueue(); + private int m_notifier; + private final double m_period; + private final long m_startTimeUs; + + // OpMode lifecycle state + private long m_lastModeId = -1; + private boolean m_calledDriverStationConnected = false; + private boolean m_lastEnabledState = false; + private OpMode m_currentOpMode; + private Callback m_currentOpModePeriodic; + private final Set m_activeOpModeCallbacks = new HashSet<>(); + private final Watchdog m_watchdog; + private final Alert m_loopOverrunAlert; private static void reportAddOpModeError(Class cls, String message) { DriverStation.reportError("Error adding OpMode " + cls.getSimpleName() + ": " + message, false); @@ -86,9 +112,12 @@ public abstract class OpModeRobot extends RobotBase { Optional> ctor; // try 2-parameter constructor - ctor = ConstructorMatch.findBestConstructor(cls, getClass(), m_userControlsInstance.getClass()); - if (ctor.isPresent()) { - return ctor; + if (m_userControlsInstance != null) { + ctor = + ConstructorMatch.findBestConstructor(cls, getClass(), m_userControlsInstance.getClass()); + if (ctor.isPresent()) { + return ctor; + } } // try 1-parameter constructor with RobotBase parameter @@ -98,18 +127,16 @@ public abstract class OpModeRobot extends RobotBase { } // try 1-parameter constructor with UserControls parameter - ctor = ConstructorMatch.findBestConstructor(cls, m_userControlsInstance.getClass()); - if (ctor.isPresent()) { - return ctor; + if (m_userControlsInstance != null) { + ctor = ConstructorMatch.findBestConstructor(cls, m_userControlsInstance.getClass()); + if (ctor.isPresent()) { + return ctor; + } } // try no-parameter constructor ctor = ConstructorMatch.findBestConstructor(cls); - if (ctor.isPresent()) { - return ctor; - } - - return Optional.empty(); + return ctor; } private T constructOpModeClass(Class cls) { @@ -120,7 +147,11 @@ public abstract class OpModeRobot extends RobotBase { return null; } try { - return constructor.get().newInstance(this, m_userControlsInstance); + if (m_userControlsInstance != null) { + return constructor.get().newInstance(this, m_userControlsInstance); + } else { + return constructor.get().newInstance(this); + } } catch (ReflectiveOperationException e) { DriverStation.reportError( "Could not instantiate OpMode " + cls.getSimpleName(), e.getStackTrace()); @@ -226,9 +257,10 @@ public abstract class OpModeRobot extends RobotBase { /** * Adds an opmode for an opmode class. The 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). It's necessary to call publishOpModes() to make the - * added mode visible to the driver station. + * with a public constructor that either takes no arguments or accepts a single argument + * assignable from this robot class type (the latter is preferred; if multiple match, the most + * specific parameter type is used). It's necessary to call publishOpModes() to make the added + * mode visible to the driver station. * * @param cls class to add * @param mode robot mode @@ -260,9 +292,10 @@ public abstract class OpModeRobot extends RobotBase { /** * Adds an opmode for an opmode class. The 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). It's necessary to call publishOpModes() to make the - * added mode visible to the driver station. + * with a public constructor that either takes no arguments or accepts a single argument + * assignable from this robot class type (the latter is preferred; if multiple match, the most + * specific parameter type is used). It's necessary to call publishOpModes() to make the added + * mode visible to the driver station. * * @param cls class to add * @param mode robot mode @@ -278,9 +311,10 @@ public abstract class OpModeRobot extends RobotBase { /** * Adds an opmode for an opmode class. The 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). It's necessary to call publishOpModes() to make the - * added mode visible to the driver station. + * with a public constructor that either takes no arguments or accepts a single argument + * assignable from this robot class type (the latter is preferred; if multiple match, the most + * specific parameter type is used). It's necessary to call publishOpModes() to make the added + * mode visible to the driver station. * * @param cls class to add * @param mode robot mode @@ -294,9 +328,10 @@ public abstract class OpModeRobot extends RobotBase { /** * Adds an opmode for an opmode class. The 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). It's necessary to call publishOpModes() to make the - * added mode visible to the driver station. + * with a public constructor that either takes no arguments or accepts a single argument + * assignable from this robot class type (the latter is preferred; if multiple match, the most + * specific parameter type is used). It's necessary to call publishOpModes() to make the added + * mode visible to the driver station. * * @param cls class to add * @param mode robot mode @@ -364,8 +399,9 @@ public abstract class OpModeRobot extends RobotBase { /** * Adds an opmode for an opmode class annotated with {@link Autonomous}, {@link Teleop}, or {@link * TestOpMode}. The 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. - * It's necessary to call publishOpModes() to make the added mode visible to the driver station. + * constructor that either takes no arguments or accepts a single argument assignable from this + * robot class type (if multiple match, the most specific parameter type is used). It's necessary + * to call publishOpModes() to make the added mode visible to the driver station. * * @param cls class to add * @throws IllegalArgumentException if class does not meet criteria @@ -485,9 +521,37 @@ public abstract class OpModeRobot extends RobotBase { m_opModes.clear(); } - /** Constructor. */ + /** Default loop period. */ + public static final double DEFAULT_PERIOD = 0.02; + + /** Constructor with default period. */ @SuppressWarnings("this-escape") public OpModeRobot() { + this(DEFAULT_PERIOD); + } + + /** + * Constructor with specified period. + * + * @param period the period at which to run the robot and opmode periodic callbacks. + */ + @SuppressWarnings("this-escape") + public OpModeRobot(double period) { + m_period = period; + + // Create our own notifier and callback queue (match C++) + m_notifier = NotifierJNI.createNotifier(); + NotifierJNI.setNotifierName(m_notifier, "OpModeRobot"); + + m_startTimeUs = RobotController.getMonotonicTime(); + + m_loopOverrunAlert = + new Alert("Loop time of \"" + m_period + "\"s overrun", Alert.Level.MEDIUM); + m_watchdog = new Watchdog(Seconds.of(m_period), () -> m_loopOverrunAlert.set(true)); + + // Add LoopFunc as periodic callback (match C++) + addPeriodic(this::loopFunc, period); + // Check to see if we have a DS annotation UserControlsInstance userControlsAnnotation = getClass().getAnnotation(UserControlsInstance.class); @@ -499,18 +563,55 @@ public abstract class OpModeRobot extends RobotBase { // Scan for annotated opmode classes within the derived class's package and subpackages addAnnotatedOpModeClasses(getClass().getPackage()); DriverStation.publishOpModes(); + + HAL.reportUsage("Framework", "OpModeRobot"); } /** - * Function called exactly once after the DS is connected. + * Add a callback to run at a specific period. * - *

Code that needs to know the DS state should go here. + * @param callback The callback to run. + * @param period The period at which to run the callback. + */ + public void addPeriodic(Runnable callback, double period) { + m_callbacks.add(callback, m_startTimeUs, period); + } + + /** + * Get the period at which robot and opmode periodic callbacks are run. + * + * @return The period at which robot and opmode periodic callbacks are run. + */ + public double getPeriod() { + return m_period; + } + + /** + * 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. */ public void driverStationConnected() {} + /** Function called periodically every loop, regardless of enabled state or OpMode selection. */ + public void robotPeriodic() {} + + /** Function called once during robot initialization in simulation. */ + public void simulationInit() {} + + /** Function called periodically in simulation. */ + public void simulationPeriodic() {} + + /** Function called once when the robot becomes disabled. */ + public void disabledInit() {} + + /** Function called periodically while the robot is disabled. */ + public void disabledPeriodic() {} + + /** Function called once when the robot exits disabled state. */ + public void disabledExit() {} + /** * Function called periodically anytime when no opmode is selected, including when the Driver * Station is disconnected. @@ -518,76 +619,142 @@ public abstract class OpModeRobot extends RobotBase { public void nonePeriodic() {} /** - * Background monitor thread. On mode/opmode change, this checks to see if the change is actually - * reflected in this class within a reasonable amount of time. If not, that means that the user - * code is stuck and we need to take action to try to get it to exit (up to and including program - * termination). + * Return the system clock time in microseconds for the start of the current periodic loop. This + * is in the same time base as Timer.getMonotonicTimestamp(), 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. */ - private void monitorThreadMain(Thread thr, long opmode, int event, int endEvent) { - ControlWord word = new ControlWord(); - int[] events = {event, endEvent}; - while (true) { - try { - int[] signaled = WPIUtilJNI.waitForObjects(events); - for (int val : signaled) { - if (val < 0) { - return; // handle destroyed + public long getLoopStartTime() { + return m_callbacks.getLoopStartTime(); + } + + /** Main robot loop function. Handles disabled state logic and opmode management. */ + private void loopFunc() { + DriverStation.refreshData(); + + // Get current enabled state and opmode + DriverStation.refreshControlWordFromCache(m_word); + m_watchdog.reset(); + boolean enabled = m_word.isEnabled(); + long modeId = m_word.isDSAttached() ? m_word.getOpModeId() : 0; + + if (!m_calledDriverStationConnected && m_word.isDSAttached()) { + m_calledDriverStationConnected = true; + driverStationConnected(); + m_watchdog.addEpoch("driverStationConnected()"); + } + + // Handle opmode changes + if (modeId != m_lastModeId) { + // Clean up current opmode + if (m_currentOpMode != null) { + // Remove opmode callbacks + m_callbacks.remove(m_currentOpModePeriodic); + m_callbacks.removeAll(m_activeOpModeCallbacks); + m_activeOpModeCallbacks.clear(); + m_currentOpMode.end(); + m_currentOpMode.close(); + m_currentOpMode = null; + } + + // Set up new opmode + if (modeId != 0) { + OpModeFactory factory = m_opModes.get(modeId); + if (factory != null) { + // Instantiate the new opmode + System.out.println("********** Starting OpMode " + factory.name() + " **********"); + m_currentOpMode = factory.supplier().get(); + if (m_currentOpMode != null) { + // Ensure disabledPeriodic is called at least once + m_currentOpMode.disabledPeriodic(); + m_watchdog.addEpoch("opMode.disabledPeriodic()"); + // Register the opmode's periodic callbacks + m_currentOpModePeriodic = + m_callbacks.add(m_currentOpMode::periodic, m_startTimeUs, m_period); + m_activeOpModeCallbacks.addAll(m_currentOpMode.getCallbacks()); + m_callbacks.addAll(m_activeOpModeCallbacks); } - } - } catch (InterruptedException e) { - Thread.currentThread().interrupt(); - return; - } - - // did the opmode or enable state change? - DriverStationJNI.getUncachedControlWord(word); - if (!word.isEnabled() || word.getOpModeId() != opmode) { - break; - } - } - - // call opmode stop - OpMode opMode = m_activeOpMode.get(); - if (opMode != null) { - opMode.opModeStop(); - } - - events[0] = m_notifier; - NotifierJNI.setNotifierAlarm(m_notifier, 200000, 0, false, true); // 200 ms - try { - int[] signaled = WPIUtilJNI.waitForObjects(events); - for (int val : signaled) { - if (val < 0 || val == endEvent) { - return; // transitioned, or handle destroyed + } else { + DriverStation.reportError("No OpMode found for mode " + modeId, false); } } - } catch (InterruptedException e) { - Thread.currentThread().interrupt(); - return; + m_lastModeId = modeId; } - // if it hasn't transitioned after 200 ms, call thread.interrupt() - DriverStation.reportError("OpMode did not exit, interrupting thread", false); - thr.interrupt(); - - NotifierJNI.setNotifierAlarm(m_notifier, 800000, 0, false, true); // 800 ms - try { - int[] signaled = WPIUtilJNI.waitForObjects(events); - for (int val : signaled) { - if (val < 0 || val == endEvent) { - return; // transitioned, or handle destroyed + // Handle enabled state changes + boolean justCalledDisabledInit = false; + if (m_lastEnabledState != enabled) { + if (enabled) { + // Transitioning to enabled + disabledExit(); + m_watchdog.addEpoch("disabledExit()"); + if (m_currentOpMode != null) { + m_currentOpMode.start(); + m_watchdog.addEpoch("opMode.start()"); } + } else { + // Transitioning to disabled + if (m_currentOpMode != null && m_lastEnabledState) { + // Was enabled, now disabled + m_currentOpMode.end(); + m_watchdog.addEpoch("opMode.end()"); + } + disabledInit(); + m_watchdog.addEpoch("disabledInit()"); + justCalledDisabledInit = true; } - } catch (InterruptedException e) { - Thread.currentThread().interrupt(); - return; + m_lastEnabledState = enabled; } - // if it hasn't transitioned after 1 second, terminate the program - DriverStation.reportError("OpMode did not exit, terminating program", false); - HAL.terminate(); - HAL.shutdown(); - System.exit(0); + // Call periodic functions based on current state + if (!enabled) { + // Only call disabledPeriodic if we didn't just call disabledInit + if (!justCalledDisabledInit) { + disabledPeriodic(); + m_watchdog.addEpoch("disabledPeriodic()"); + } + + // Call opmode disabledPeriodic if we have one + if (m_currentOpMode != null) { + m_currentOpMode.disabledPeriodic(); + m_watchdog.addEpoch("opMode.disabledPeriodic()"); + } + } + + // Call nonePeriodic when no opmode is selected + if (DriverStation.getOpModeId() == 0) { + nonePeriodic(); + m_watchdog.addEpoch("nonePeriodic()"); + } + + // Always call robotPeriodic + robotPeriodic(); + m_watchdog.addEpoch("robotPeriodic()"); + + // Always observe user program state + DriverStationJNI.observeUserProgram(m_word.getNative()); + + SmartDashboard.updateValues(); + m_watchdog.addEpoch("SmartDashboard.updateValues()"); + + // Call simulationPeriodic if in simulation + if (isSimulation()) { + HAL.simPeriodicBefore(); + simulationPeriodic(); + HAL.simPeriodicAfter(); + m_watchdog.addEpoch("simulationPeriodic()"); + } + + m_watchdog.disable(); + + // Flush NetworkTables + NetworkTableInstance.getDefault().flushLocal(); + + // Warn on loop time overruns + if (m_watchdog.isExpired()) { + m_watchdog.printEpochs(); + } } /** Provide an alternate "main loop" via startCompetition(). */ @@ -595,145 +762,34 @@ public abstract class OpModeRobot extends RobotBase { public final void startCompetition() { System.out.println("********** Robot program startup complete **********"); - int event = WPIUtilJNI.makeEvent(false, false); - DriverStationJNI.provideNewDataEventHandle(event); - - m_notifier = NotifierJNI.createNotifier(); - NotifierJNI.setNotifierName(m_notifier, "OpModeRobot"); - - try { - // Implement the opmode lifecycle - long lastModeId = -1; - boolean calledObserveUserProgramStarting = false; - boolean calledDriverStationConnected = false; - int[] events = {event, m_notifier}; - while (true) { - // Wait for new data from the driver station, with 50 ms timeout - NotifierJNI.setNotifierAlarm(m_notifier, 50000, 0, false, true); - - // Call observeUserProgramStarting() here as a one-shot to ensure it is called after the - // notifier alarm is set. The notifier alarm is set using relative time, so tests that - // wait on the user program to start and then step time won't work correctly if we call - // this before setting the alarm. - if (!calledObserveUserProgramStarting) { - calledObserveUserProgramStarting = true; - DriverStation.observeUserProgramStarting(); - } - - try { - int[] signaled = WPIUtilJNI.waitForObjects(events); - for (int val : signaled) { - if (val < 0) { - return; // handle destroyed - } - } - } catch (InterruptedException ex) { - Thread.currentThread().interrupt(); - break; - } - - // Get the latest control word and opmode - DriverStation.refreshData(); - DriverStation.refreshControlWordFromCache(m_word); - - if (!calledDriverStationConnected && m_word.isDSAttached()) { - calledDriverStationConnected = true; - driverStationConnected(); - } - - long modeId; - if (!m_word.isDSAttached()) { - modeId = 0; - } else { - modeId = m_word.getOpModeId(); - } - - OpMode opMode = m_activeOpMode.get(); - if (opMode == null || modeId != lastModeId) { - if (opMode != null) { - // no or different opmode selected - m_activeOpMode.set(null); - opMode.opModeClose(); - } - - if (modeId == 0) { - // no opmode selected - nonePeriodic(); - DriverStationJNI.observeUserProgram(m_word.getNative()); - continue; - } - - OpModeFactory factory = m_opModes.get(modeId); - if (factory == null) { - DriverStation.reportError("No OpMode found for mode " + modeId, false); - m_word.setOpModeId(0); - DriverStationJNI.observeUserProgram(m_word.getNative()); - continue; - } - - // Instantiate the opmode - System.out.println("********** Starting OpMode " + factory.name() + " **********"); - opMode = factory.supplier().get(); - if (opMode == null) { - // could not construct - m_word.setOpModeId(0); - DriverStationJNI.observeUserProgram(m_word.getNative()); - continue; - } - m_activeOpMode.set(opMode); - lastModeId = modeId; - // Ensure disabledPeriodic is always called at least once - opMode.disabledPeriodic(); - } - - DriverStationJNI.observeUserProgram(m_word.getNative()); - - if (m_word.isEnabled()) { - // When enabled, call the opmode run function, then close and clear - int endMonitor = WPIUtilJNI.makeEvent(true, false); - Thread curThread = Thread.currentThread(); - Thread monitor = - new Thread( - () -> { - monitorThreadMain(curThread, modeId, event, endMonitor); - }); - monitor.start(); - try { - opMode.opModeRun(modeId); - } catch (InterruptedException e) { - // ignored - } finally { - Thread.interrupted(); - WPIUtilJNI.destroyEvent(endMonitor); - try { - monitor.join(); - } catch (InterruptedException e) { - Thread.currentThread().interrupt(); - } - } - opMode = m_activeOpMode.getAndSet(null); - if (opMode != null) { - opMode.opModeClose(); - } - } else { - // When disabled, call the disabledPeriodic function - opMode.disabledPeriodic(); - } - } - } finally { - DriverStationJNI.removeNewDataEventHandle(event); - WPIUtilJNI.destroyEvent(event); - NotifierJNI.destroyNotifier(m_notifier); + if (isSimulation()) { + simulationInit(); } + + // Tell the DS that the robot is ready to be enabled + DriverStation.observeUserProgramStarting(); + + // Loop forever, calling the callback system which handles periodic functions + while (true) { + if (!m_callbacks.runCallbacks(m_notifier)) { + break; + } + } + } + + @Override + public void close() { + NotifierJNI.destroyNotifier(m_notifier); } /** Ends the main loop in startCompetition(). */ @Override public final void endCompetition() { NotifierJNI.destroyNotifier(m_notifier); - OpMode opMode = m_activeOpMode.get(); - if (opMode != null) { - opMode.opModeStop(); - } + } + + /** Prints list of epochs added so far and their times. */ + public void printWatchdogEpochs() { + m_watchdog.printEpochs(); } } diff --git a/wpilibj/src/main/java/org/wpilib/framework/TimedRobot.java b/wpilibj/src/main/java/org/wpilib/framework/TimedRobot.java index 9bb7af9e5f..ae73d1b3f5 100644 --- a/wpilibj/src/main/java/org/wpilib/framework/TimedRobot.java +++ b/wpilibj/src/main/java/org/wpilib/framework/TimedRobot.java @@ -6,14 +6,13 @@ package org.wpilib.framework; import static org.wpilib.units.Units.Seconds; -import java.util.PriorityQueue; import org.wpilib.driverstation.DriverStation; import org.wpilib.hardware.hal.HAL; import org.wpilib.hardware.hal.NotifierJNI; +import org.wpilib.internal.PeriodicPriorityQueue; import org.wpilib.system.RobotController; import org.wpilib.units.measure.Frequency; import org.wpilib.units.measure.Time; -import org.wpilib.util.WPIUtilJNI; /** * TimedRobot implements the IterativeRobotBase robot program framework. @@ -23,59 +22,17 @@ import org.wpilib.util.WPIUtilJNI; *

periodic() functions from the base class are called on an interval by a Notifier instance. */ public class TimedRobot extends IterativeRobotBase { - @SuppressWarnings("MemberName") - static class Callback implements Comparable { - public Runnable func; - public long period; - public long expirationTime; - - /** - * Construct a callback container. - * - * @param func The callback to run. - * @param startTime The common starting point for all callback scheduling in microseconds. - * @param period The period at which to run the callback in microseconds. - * @param offset The offset from the common starting time in microseconds. - */ - Callback(Runnable func, long startTime, long period, long offset) { - this.func = func; - this.period = period; - this.expirationTime = - startTime - + offset - + this.period - + (RobotController.getMonotonicTime() - startTime) / this.period * this.period; - } - - @Override - public boolean equals(Object rhs) { - return rhs instanceof Callback callback && expirationTime == callback.expirationTime; - } - - @Override - public int hashCode() { - return Long.hashCode(expirationTime); - } - - @Override - public int compareTo(Callback rhs) { - // Elements with sooner expiration times are sorted as lesser. The head of - // Java's PriorityQueue is the least element. - return Long.compare(expirationTime, rhs.expirationTime); - } - } - /** Default loop period. */ + @SuppressWarnings("MemberName") public static final double DEFAULT_PERIOD = 0.02; // The C pointer to the notifier object. We don't use it directly, it is // just passed to the JNI bindings. private final int m_notifier = NotifierJNI.createNotifier(); - private long m_startTimeUs; - private long m_loopStartTimeUs; + private final long m_startTimeUs; - private final PriorityQueue m_callbacks = new PriorityQueue<>(); + private final PeriodicPriorityQueue m_callbackQueue = new PeriodicPriorityQueue(); /** Constructor for TimedRobot. */ protected TimedRobot() { @@ -87,6 +44,7 @@ public class TimedRobot extends IterativeRobotBase { * * @param period The period of the robot loop function. */ + @SuppressWarnings("this-escape") protected TimedRobot(double period) { super(period); m_startTimeUs = RobotController.getMonotonicTime(); @@ -132,45 +90,9 @@ public class TimedRobot extends IterativeRobotBase { // Loop forever, calling the appropriate mode-dependent function while (true) { - // 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. - var callback = m_callbacks.poll(); - - NotifierJNI.setNotifierAlarm(m_notifier, callback.expirationTime, 0, true, true); - - try { - WPIUtilJNI.waitForObject(m_notifier); - } catch (InterruptedException ex) { - Thread.currentThread().interrupt(); + if (!m_callbackQueue.runCallbacks(m_notifier)) { break; } - - long currentTime = RobotController.getMonotonicTime(); - m_loopStartTimeUs = currentTime; - - callback.func.run(); - - // 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.add(callback); - - // Process all other callbacks that are ready to run - while (m_callbacks.peek().expirationTime <= currentTime) { - callback = m_callbacks.poll(); - - callback.func.run(); - - callback.expirationTime += - callback.period - + (currentTime - callback.expirationTime) / callback.period * callback.period; - m_callbacks.add(callback); - } } } @@ -188,7 +110,7 @@ public class TimedRobot extends IterativeRobotBase { * @return Robot running time in microseconds, as of the start of the current periodic function. */ public long getLoopStartTime() { - return m_loopStartTimeUs; + return m_callbackQueue.getLoopStartTime(); } /** @@ -201,7 +123,7 @@ public class TimedRobot extends IterativeRobotBase { * @param period The period at which to run the callback in seconds. */ public final void addPeriodic(Runnable callback, double period) { - m_callbacks.add(new Callback(callback, m_startTimeUs, (long) (period * 1e6), 0)); + addPeriodic(callback, period, period); } /** @@ -216,35 +138,6 @@ public class TimedRobot extends IterativeRobotBase { * scheduling a callback in a different timeslot relative to TimedRobot. */ public final void addPeriodic(Runnable callback, double period, double offset) { - m_callbacks.add( - new Callback(callback, m_startTimeUs, (long) (period * 1e6), (long) (offset * 1e6))); - } - - /** - * Add a callback to run at a specific period. - * - *

This is scheduled on TimedRobot's Notifier, so TimedRobot 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. - */ - public final void addPeriodic(Runnable callback, Time period) { - addPeriodic(callback, period.in(Seconds)); - } - - /** - * Add a callback to run at a specific period with a starting time offset. - * - *

This is scheduled on TimedRobot's Notifier, so TimedRobot 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. - */ - public final void addPeriodic(Runnable callback, Time period, Time offset) { - addPeriodic(callback, period.in(Seconds), offset.in(Seconds)); + m_callbackQueue.add(callback, m_startTimeUs, period, offset); } } diff --git a/wpilibj/src/main/java/org/wpilib/internal/PeriodicPriorityQueue.java b/wpilibj/src/main/java/org/wpilib/internal/PeriodicPriorityQueue.java new file mode 100644 index 0000000000..2d4eeca958 --- /dev/null +++ b/wpilibj/src/main/java/org/wpilib/internal/PeriodicPriorityQueue.java @@ -0,0 +1,312 @@ +// 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. + +package org.wpilib.internal; + +import java.util.Collection; +import java.util.PriorityQueue; +import org.wpilib.hardware.hal.NotifierJNI; +import org.wpilib.system.RobotController; +import org.wpilib.util.WPIUtilJNI; + +/** + * A priority queue for scheduling periodic callbacks based on their next execution time. + * + *

This class manages a collection of periodic callbacks that execute at specified intervals. + * Callbacks are scheduled using monotonic timestamps and automatically rescheduled after execution + * to maintain their periodic behavior. The queue uses a priority heap to efficiently determine the + * next callback to execute. + * + *

This is an internal scheduling primitive used by robot frameworks like TimedRobot. + */ +public class PeriodicPriorityQueue { + /** Internal priority queue ordered by callback expiration times. */ + private final PriorityQueue m_queue; + + private long m_loopStartTimeMicros; + + /** Constructs an empty callback queue. */ + public PeriodicPriorityQueue() { + m_queue = new PriorityQueue<>(); + } + + /** + * Adds a periodic callback to the queue with a specified start time. + * + * @param func The function to call periodically. + * @param timestamp The common starting point for callback scheduling in monotonic timestamp + * microseconds. + * @param periodSeconds The callback period in seconds. + * @param offsetSeconds The offset from the common starting time in seconds. + * @return the callback object + */ + public Callback add(Runnable func, long timestamp, double periodSeconds, double offsetSeconds) { + Callback callback = new Callback(func, timestamp, periodSeconds, offsetSeconds); + add(callback); + return callback; + } + + /** + * Adds a periodic callback to the queue with a specified start time. + * + * @param func The function to call periodically. + * @param timestamp The common starting point for callback scheduling in monotonic timestamp + * microseconds. + * @param periodSeconds The callback period in seconds. + * @return the callback object + */ + public Callback add(Runnable func, long timestamp, double periodSeconds) { + Callback callback = new Callback(func, timestamp, periodSeconds); + m_queue.add(callback); + return callback; + } + + /** + * Adds a periodic callback to the queue, starting from the current monotonic time. + * + * @param func The function to call periodically. + * @param periodSeconds The callback period in seconds. + * @param offsetSeconds The offset from the current monotonic time in seconds. + * @return the callback object + */ + public Callback add(Runnable func, double periodSeconds, double offsetSeconds) { + return add(func, RobotController.getMonotonicTime(), periodSeconds, offsetSeconds); + } + + /** + * Adds a periodic callback to the queue, starting from the current monotonic time. + * + * @param func The function to call periodically. + * @param periodSeconds The callback period in seconds. + * @return the callback object + */ + public Callback add(Runnable func, double periodSeconds) { + return add(func, RobotController.getMonotonicTime(), periodSeconds); + } + + /** + * Adds a pre-constructed callback to the queue. + * + * @param callback The callback to add. + */ + public void add(Callback callback) { + m_queue.add(callback); + } + + /** + * Adds multiple callbacks to the queue. + * + * @param callbacks The collection of callbacks to add. + */ + public void addAll(Collection callbacks) { + m_queue.addAll(callbacks); + } + + /** + * Removes all callbacks associated with the given function. + * + * @param func The function whose callbacks should be removed. + */ + public void remove(Runnable func) { + m_queue.removeIf(callback -> callback.m_func.equals(func)); + } + + /** + * Removes a specific callback from the queue. + * + * @param callback The callback to remove. + */ + public void remove(Callback callback) { + m_queue.remove(callback); + } + + /** + * Removes multiple callbacks from the queue. + * + * @param callbacks The collection of callbacks to remove. + */ + public void removeAll(Collection callbacks) { + m_queue.removeAll(callbacks); + } + + /** Removes all callbacks from the queue. */ + public void clear() { + m_queue.clear(); + } + + /** + * Executes all callbacks that are due, then waits for the next callback's scheduled time. + * + *

This method performs the following steps: + * + *

    + *
  1. Retrieves the callback with the earliest expiration time from the queue + *
  2. Sets a hardware notifier alarm to wait until that callback's expiration time + *
  3. Blocks until the notifier signals or is interrupted + *
  4. Executes the callback and reschedules it for its next period + *
  5. Processes any additional callbacks that have become due during execution + *
+ * + *

When rescheduling callbacks, this method automatically compensates for execution delays by + * advancing the expiration time by the number of full periods that have elapsed, ensuring + * callbacks maintain their scheduled phase over time. + * + * @param notifier The HAL notifier handle to use for timing. + * @return whether the notifier was signaled before the timeout. + * @throws IllegalStateException if the queue is empty when this method is called. + */ + public boolean runCallbacks(int notifier) { + var callback = m_queue.poll(); + if (callback == null) { + throw new IllegalStateException( + "No callbacks to run! Did you make sure to call add() first?"); + } + + NotifierJNI.setNotifierAlarm(notifier, callback.m_expirationTime, 0, true, true); + + try { + WPIUtilJNI.waitForObject(notifier); + } catch (InterruptedException ex) { + return false; + } + + m_loopStartTimeMicros = RobotController.getMonotonicTime(); + + callback.m_func.run(); + + // 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 m_loopStartTime ≥ expirationTime rather than checking for it since + // the callback wouldn't be running otherwise. + callback.m_expirationTime += + callback.m_period + + (m_loopStartTimeMicros - callback.m_expirationTime) + / callback.m_period + * callback.m_period; + m_queue.add(callback); + + // Process all other callbacks that are ready to run + while (m_queue.peek().m_expirationTime <= m_loopStartTimeMicros) { + callback = m_queue.poll(); + + callback.m_func.run(); + + callback.m_expirationTime += + callback.m_period + + (m_loopStartTimeMicros - callback.m_expirationTime) + / callback.m_period + * callback.m_period; + m_queue.add(callback); + } + + return true; + } + + /** + * Return the system clock time in microseconds for the start of the current periodic loop. This + * is in the same time base as Timer.getMonotonicTimeStamp(), 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. + */ + public long getLoopStartTime() { + return m_loopStartTimeMicros; + } + + /** + * A periodic callback with scheduling metadata. + * + *

Each callback tracks its target function, period, and next expiration time. After execution, + * the expiration time is automatically advanced by full periods to maintain precise timing even + * when execution is delayed. + */ + public static class Callback implements Comparable { + /** The function to execute when the callback fires. */ + public final Runnable m_func; + + /** The period at which to run the callback in microseconds. */ + public final long m_period; + + /** The next scheduled execution time in monotonic timestamp microseconds. */ + public long m_expirationTime; + + /** + * Construct a callback container. + * + * @param func The callback to run. + * @param startTime The common starting point for all callback scheduling in microseconds. + * @param period The period at which to run the callback in microseconds. + * @param offset The offset from the common starting time in microseconds. + */ + public Callback(Runnable func, long startTime, long period, long offset) { + this.m_func = func; + this.m_period = period; + this.m_expirationTime = + startTime + + offset + + (1 + (RobotController.getMonotonicTime() - startTime - offset) / this.m_period) + * this.m_period; + } + + /** + * Construct a callback container. + * + * @param func The callback to run. + * @param timestamp The common starting point for all callback scheduling in microseconds. + * @param periodSeconds The period at which to run the callback in seconds. + * @param offsetSeconds The offset from the common starting time in seconds. + */ + public Callback(Runnable func, long timestamp, double periodSeconds, double offsetSeconds) { + this(func, timestamp, (long) (periodSeconds * 1e6), (long) (offsetSeconds * 1e6)); + } + + /** + * Construct a callback container. + * + * @param func The callback to run. + * @param timestamp The common starting point for all callback scheduling in microseconds. + * @param periodSeconds The period at which to run the callback in seconds. + */ + public Callback(Runnable func, long timestamp, double periodSeconds) { + this(func, timestamp, (long) (periodSeconds * 1e6), 0); + } + + /** + * Compares callbacks based on expiration time for equality. + * + * @param rhs The object to compare against. + * @return true if rhs is a Callback with the same expiration time. + */ + @Override + public boolean equals(Object rhs) { + return rhs instanceof Callback callback && m_expirationTime == callback.m_expirationTime; + } + + /** + * Returns a hash code based on the expiration time. + * + * @return hash code for this callback. + */ + @Override + public int hashCode() { + return Long.hashCode(m_expirationTime); + } + + /** + * Compares this callback to another based on expiration time. + * + *

Callbacks with earlier expiration times are considered "less than" those with later + * expiration times. This ordering is used by the priority queue to determine execution order. + * + * @param rhs The callback to compare to. + * @return negative if this expires before rhs, positive if after, zero if equal. + */ + @Override + public int compareTo(Callback rhs) { + // Elements with sooner expiration times are sorted as lesser. The head of + // Java's PriorityQueue is the least element. + return Long.compare(m_expirationTime, rhs.m_expirationTime); + } + } +} diff --git a/wpilibj/src/main/java/org/wpilib/opmode/OpMode.java b/wpilibj/src/main/java/org/wpilib/opmode/OpMode.java index 4c71f0fb15..00427992f3 100644 --- a/wpilibj/src/main/java/org/wpilib/opmode/OpMode.java +++ b/wpilibj/src/main/java/org/wpilib/opmode/OpMode.java @@ -4,36 +4,81 @@ package org.wpilib.opmode; +import java.util.Set; +import org.wpilib.framework.OpModeRobot; +import org.wpilib.internal.PeriodicPriorityQueue; + /** * Top-level interface for opmode classes. Users should generally extend one of the abstract * implementations of this interface (e.g. {@link PeriodicOpMode}) rather than directly implementing * this interface. + * + *

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 at {@link OpModeRobot#getPeriod}, + * 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 close(); the object is + * not reused + *
  • if a different opmode is selected on the driver station when the DS is disabled, only + * close() is called; the object is not reused + *
+ * + *

All lifecycle callbacks and periodic callbacks run synchronously on the same thread that + * invokes them. Interactions between opmodes and the robot framework do not require additional + * synchronization. + * + *

Additional callbacks can be registered by implementing {@link #getCallbacks()} to return a set + * of {@link PeriodicPriorityQueue.Callback} objects with custom timing. {@link PeriodicOpMode} + * provides a convenient implementation of this method and utility methods for adding periodic + * callbacks. */ -public interface OpMode { +public interface OpMode extends AutoCloseable { /** - * 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. + * This function is called periodically while the opmode is selected and the robot is disabled. + * Code that should only run once when the opmode is selected should go in the opmode constructor. */ default void disabledPeriodic() {} + /** Called once when this opmode transitions to enabled. */ + default void start() {} + /** - * This function is called when the opmode starts (robot is enabled). + * This function is called periodically while the opmode is enabled at the rate returned by {@link + * OpModeRobot#getPeriod()}. + */ + default void periodic() {} + + /** + * This function is called asynchronously when the robot disables or switches opmodes while this + * opmode is enabled. Implementations should stop blocking work promptly. + */ + default void end() {} + + /** + * This function is called when the opmode is no longer selected on the DS or after an enabled run + * ends. The object will not be reused after this is called. + */ + @Override + default void close() {} + + /** + * Returns a set of custom periodic callbacks to be executed while the opmode is enabled. * - * @param opModeId opmode unique ID - * @throws InterruptedException when interrupted + *

This method allows opmodes to register arbitrary periodic callbacks with custom execution + * intervals. The callbacks are executed by the robot framework at their scheduled times, in + * addition to the primary {@link #periodic()} callback. + * + * @return A set of custom callbacks to execute, or an empty set if no custom callbacks are + * needed. The default implementation returns an empty set. */ - void opModeRun(long opModeId) throws InterruptedException; - - /** - * This function is called asynchronously when the robot is disabled, to request the opmode return - * from opModeRun(). - */ - void opModeStop(); - - /** - * This function is called when the opmode is no longer selected on the DS or after opModeRun() - * returns. The object will not be reused after this is called. - */ - void opModeClose(); + default Set getCallbacks() { + return Set.of(); + } } diff --git a/wpilibj/src/main/java/org/wpilib/opmode/PeriodicOpMode.java b/wpilibj/src/main/java/org/wpilib/opmode/PeriodicOpMode.java index fa737c8454..bc99d267ff 100644 --- a/wpilibj/src/main/java/org/wpilib/opmode/PeriodicOpMode.java +++ b/wpilibj/src/main/java/org/wpilib/opmode/PeriodicOpMode.java @@ -4,28 +4,20 @@ package org.wpilib.opmode; -import static org.wpilib.units.Units.Seconds; - -import java.util.PriorityQueue; -import org.wpilib.driverstation.DriverStation; -import org.wpilib.hardware.hal.ControlWord; -import org.wpilib.hardware.hal.DriverStationJNI; +import java.util.Collections; +import java.util.Set; +import java.util.TreeSet; import org.wpilib.hardware.hal.HAL; -import org.wpilib.hardware.hal.NotifierJNI; -import org.wpilib.networktables.NetworkTableInstance; -import org.wpilib.smartdashboard.SmartDashboard; +import org.wpilib.internal.PeriodicPriorityQueue; import org.wpilib.system.RobotController; -import org.wpilib.system.Watchdog; -import org.wpilib.units.measure.Time; -import org.wpilib.util.WPIUtilJNI; /** * 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 abstract 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. + * ms by default, but may be changed via passing a different time interval to OpModeRobot's + * constructor. Additional periodic callbacks with different intervals can be added using the + * addPeriodic() set of functions. * *

Lifecycle: * @@ -34,311 +26,61 @@ import org.wpilib.util.WPIUtilJNI; *

  • 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 + *
  • while DS is enabled, periodic() is called periodically on the time interval set by + * OpModeRobot's 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 close(); the object is * not reused *
  • if a different opmode is selected on the driver station when the DS is disabled, only * close() is called; the object is not reused * + * + *

    All lifecycle callbacks and periodic callbacks run synchronously on the same thread that + * invokes them. Interactions between opmodes and the robot framework do not require additional + * synchronization. */ public abstract class PeriodicOpMode implements OpMode { - @SuppressWarnings("MemberName") - static class Callback implements Comparable { - public Runnable func; - public long period; - public long expirationTime; + private final Set m_callbacks; + private final long m_startTimeUs = RobotController.getMonotonicTime(); - /** - * Construct a callback container. - * - * @param func The callback to run. - * @param startTime The common starting point for all callback scheduling in microseconds. - * @param period The period at which to run the callback in microseconds. - * @param offset The offset from the common starting time in microseconds. - */ - Callback(Runnable func, long startTime, long period, long offset) { - this.func = func; - this.period = period; - this.expirationTime = - startTime - + offset - + this.period - + (RobotController.getMonotonicTime() - startTime) / this.period * this.period; - } - - @Override - public boolean equals(Object rhs) { - return rhs instanceof Callback callback && expirationTime == callback.expirationTime; - } - - @Override - public int hashCode() { - return Long.hashCode(expirationTime); - } - - @Override - public int compareTo(Callback rhs) { - // Elements with sooner expiration times are sorted as lesser. The head of - // Java's PriorityQueue is the least element. - return Long.compare(expirationTime, rhs.expirationTime); - } - } - - /** Default loop period. */ - public static final double DEFAULT_PERIOD = 0.02; - - // The C pointer to the notifier object. We don't use it directly, it is - // just passed to the JNI bindings. - private int m_notifier = NotifierJNI.createNotifier(); - - private long m_startTimeUs; - private long m_loopStartTimeUs; - - private final ControlWord m_word = new ControlWord(); - private final double m_period; - private final Watchdog m_watchdog; - - private long m_opModeId; - private boolean m_running = true; - - private final PriorityQueue m_callbacks = new PriorityQueue<>(); - - /** - * Constructor. Periodic opmodes may specify the period used for the periodic() function; the - * no-argument constructor uses a default period of 20 ms. - */ + /** Constructor for PeriodicOpMode. */ + @SuppressWarnings("this-escape") protected PeriodicOpMode() { - this(DEFAULT_PERIOD); - } - - /** - * Constructor. Periodic opmodes may specify the period used for the periodic() function. - * - * @param period period (in seconds) for callbacks to the periodic() function - */ - protected PeriodicOpMode(double period) { - m_startTimeUs = RobotController.getMonotonicTime(); - m_period = period; - m_watchdog = new Watchdog(period, this::printLoopOverrunMessage); - - addPeriodic(this::loopFunc, period); - NotifierJNI.setNotifierName(m_notifier, "PeriodicOpMode"); - + m_callbacks = new TreeSet<>(); HAL.reportUsage("OpMode", "PeriodicOpMode"); } - /** Called periodically while the opmode is selected on the DS (robot is disabled). */ @Override - public void disabledPeriodic() {} - - /** - * Called when the opmode is de-selected on the DS. The object is not reused even if the same - * opmode is selected again (a new object will be created). - */ - public void close() {} - - /** - * Called a single time when the robot transitions from disabled to enabled. This is called prior - * to periodic() being called. - */ - public void start() {} - - /** Called periodically while the robot is enabled. */ - public abstract void periodic(); - - /** - * Called a single time when the robot transitions from enabled to disabled, or just before - * close() is called if a different opmode is selected while the robot is enabled. - */ - public void end() {} - - /** - * Return the system clock time in micrseconds for the start of the current periodic loop. This is - * in the same time base as Timer.getMonotonicTimestamp(), 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. - */ - public long getLoopStartTime() { - return m_loopStartTimeUs; + public Set getCallbacks() { + return Collections.unmodifiableSet(m_callbacks); } /** * Add a callback to run at a specific period. * - *

    This is scheduled on the same Notifier as periodic(), so periodic() and the callback run + *

    This is scheduled on OpModeRobot's Notifier, so OpModeRobot 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 in seconds. */ public final void addPeriodic(Runnable callback, double period) { - m_callbacks.add(new Callback(callback, m_startTimeUs, (long) (period * 1e6), 0)); + addPeriodic(callback, period, period); } /** * 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 + *

    This is scheduled on TimedRobot's Notifier, so TimedRobot 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 in seconds. * @param offset The offset from the common starting time in seconds. This is useful for - * scheduling a callback in a different timeslot relative to PeriodicOpMode. + * scheduling a callback in a different timeslot relative to TimedRobot. */ public final void addPeriodic(Runnable callback, double period, double offset) { - m_callbacks.add( - new Callback(callback, m_startTimeUs, (long) (period * 1e6), (long) (offset * 1e6))); - } - - /** - * Add a callback to run at a specific period. - * - *

    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. - */ - public final void addPeriodic(Runnable callback, Time period) { - addPeriodic(callback, period.in(Seconds)); - } - - /** - * 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 PeriodicOpMode. - */ - public final void addPeriodic(Runnable callback, Time period, Time offset) { - addPeriodic(callback, period.in(Seconds), offset.in(Seconds)); - } - - /** - * Gets time period between calls to Periodic() functions. - * - * @return The time period between calls to Periodic() functions. - */ - public double getPeriod() { - return m_period; - } - - /** Loop function. */ - protected void loopFunc() { - DriverStation.refreshData(); - DriverStation.refreshControlWordFromCache(m_word); - m_word.setOpModeId(m_opModeId); - DriverStationJNI.observeUserProgram(m_word.getNative()); - - 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 (isSimulation()) { - // HAL.simPeriodicBefore(); - // simulationPeriodic(); - // HAL.simPeriodicAfter(); - // m_watchdog.addEpoch("simulationPeriodic()"); - // } - - m_watchdog.disable(); - - // Flush NetworkTables - NetworkTableInstance.getDefault().flushLocal(); - - // Warn on loop time overruns - if (m_watchdog.isExpired()) { - m_watchdog.printEpochs(); - } - } - - // implements OpMode interface - @Override - public final void opModeRun(long 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. - var callback = m_callbacks.poll(); - NotifierJNI.setNotifierAlarm(m_notifier, callback.expirationTime, 0, true, true); - - try { - WPIUtilJNI.waitForObject(m_notifier); - } catch (InterruptedException ex) { - Thread.currentThread().interrupt(); - break; - } - - long currentTime = RobotController.getMonotonicTime(); - m_loopStartTimeUs = RobotController.getMonotonicTime(); - - callback.func.run(); - - // 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.add(callback); - - // Process all other callbacks that are ready to run - while (m_callbacks.peek().expirationTime <= currentTime) { - callback = m_callbacks.poll(); - - callback.func.run(); - - callback.expirationTime += - callback.period - + (currentTime - callback.expirationTime) / callback.period * callback.period; - m_callbacks.add(callback); - } - } - end(); - } - - @Override - public final void opModeStop() { - NotifierJNI.destroyNotifier(m_notifier); - m_notifier = 0; - } - - @Override - public final void opModeClose() { - if (m_notifier != 0) { - NotifierJNI.destroyNotifier(m_notifier); - } - close(); - } - - /** Prints list of epochs added so far and their times. */ - public void printWatchdogEpochs() { - m_watchdog.printEpochs(); - } - - private void printLoopOverrunMessage() { - DriverStation.reportWarning("Loop time of " + m_period + "s overrun\n", false); + m_callbacks.add(new PeriodicPriorityQueue.Callback(callback, m_startTimeUs, period, offset)); } } diff --git a/wpilibj/src/test/java/org/wpilib/framework/OpModeRobotTest.java b/wpilibj/src/test/java/org/wpilib/framework/OpModeRobotTest.java new file mode 100644 index 0000000000..10820ccfce --- /dev/null +++ b/wpilibj/src/test/java/org/wpilib/framework/OpModeRobotTest.java @@ -0,0 +1,283 @@ +// 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. + +package org.wpilib.framework; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertNotEquals; +import static org.junit.jupiter.api.Assertions.fail; + +import java.util.Arrays; +import java.util.concurrent.atomic.AtomicInteger; +import org.junit.jupiter.api.AfterEach; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; +import org.junit.jupiter.api.parallel.ResourceLock; +import org.wpilib.driverstation.DriverStation; +import org.wpilib.hardware.hal.RobotMode; +import org.wpilib.opmode.OpMode; +import org.wpilib.simulation.DriverStationSim; +import org.wpilib.simulation.SimHooks; +import org.wpilib.util.Color; + +@ResourceLock("timing") +class OpModeRobotTest { + static final double kPeriod = 0.02; + + public static class MockOpMode implements OpMode { + public final AtomicInteger m_disabledPeriodicCount = new AtomicInteger(0); + public final AtomicInteger m_startCount = new AtomicInteger(0); + public final AtomicInteger m_periodicCount = new AtomicInteger(0); + public final AtomicInteger m_endCount = new AtomicInteger(0); + public final AtomicInteger m_closeCount = new AtomicInteger(0); + + MockOpMode() {} + + @Override + public void close() { + m_closeCount.incrementAndGet(); + } + + @Override + public void disabledPeriodic() { + m_disabledPeriodicCount.incrementAndGet(); + } + + @Override + public void start() { + m_startCount.incrementAndGet(); + } + + @Override + public void periodic() { + m_periodicCount.incrementAndGet(); + } + + @Override + public void end() { + m_endCount.incrementAndGet(); + } + } + + public static class OneArgOpMode implements OpMode { + @SuppressWarnings("unused") + OneArgOpMode(MockRobot robot) {} + } + + static class MockRobot extends OpModeRobot { + public final AtomicInteger m_driverStationConnectedCount = new AtomicInteger(0); + public final AtomicInteger m_nonePeriodicCount = new AtomicInteger(0); + public final AtomicInteger m_robotPeriodicCount = new AtomicInteger(0); + + MockRobot() { + super(); + } + + @Override + public void driverStationConnected() { + m_driverStationConnectedCount.incrementAndGet(); + } + + @Override + public void nonePeriodic() { + m_nonePeriodicCount.incrementAndGet(); + } + + @Override + public void robotPeriodic() { + m_robotPeriodicCount.incrementAndGet(); + } + } + + @BeforeEach + void setUp() { + SimHooks.pauseTiming(); + SimHooks.setProgramStarted(false); + DriverStationSim.resetData(); + } + + @AfterEach + void tearDown() { + DriverStation.clearOpModes(); + SimHooks.resumeTiming(); + } + + @AfterEach + @SuppressWarnings("PMD.AvoidAccessibilityAlteration") + void resetUserProgramFlag() throws ReflectiveOperationException { + var field = DriverStation.class.getDeclaredField("m_userProgramStarted"); + field.setAccessible(true); + field.set(null, false); + } + + @Test + void addOpMode() { + class MyMockRobot extends MockRobot { + MyMockRobot() { + addOpModeFactory( + () -> new MockOpMode(), + RobotMode.AUTONOMOUS, + "NoArgOpMode-Auto", + "Group", + "Description", + Color.WHITE, + Color.BLACK); + addOpModeFactory( + () -> new OneArgOpMode(this), + RobotMode.TEST, + "OneArgOpMode-Test", + "Group", + "Description", + Color.WHITE, + Color.BLACK); + addOpModeFactory(() -> new MockOpMode(), RobotMode.TELEOPERATED, "NoArgOpMode"); + addOpModeFactory(() -> new OneArgOpMode(this), RobotMode.TELEOPERATED, "OneArgOpMode"); + publishOpModes(); + } + } + + final MyMockRobot robot = new MyMockRobot(); + + var options = Arrays.asList(DriverStationSim.getOpModeOptions()); + assertEquals(4, options.size()); + + int[] indexes = {-1, -1, -1, -1}; + for (int i = 0; i < options.size(); i++) { + String name = options.get(i).name; + switch (name) { + case "NoArgOpMode-Auto" -> indexes[0] = i; + case "OneArgOpMode-Test" -> indexes[1] = i; + case "NoArgOpMode" -> indexes[2] = i; + case "OneArgOpMode" -> indexes[3] = i; + default -> fail("Unexpected op mode: " + name + " at index " + i); + } + } + + int i = indexes[0]; + assertNotEquals(-1, i); + assertEquals("Group", options.get(i).group); + assertEquals("Description", options.get(i).description); + assertEquals(0xffffff, options.get(i).textColor); + assertEquals(0x000000, options.get(i).backgroundColor); + + i = indexes[1]; + assertNotEquals(-1, i); + assertEquals("Group", options.get(i).group); + assertEquals("Description", options.get(i).description); + assertEquals(0xffffff, options.get(i).textColor); + assertEquals(0x000000, options.get(i).backgroundColor); + + i = indexes[2]; + assertNotEquals(-1, i); + assertEquals("", options.get(i).group); + assertEquals("", options.get(i).description); + assertEquals(-1, options.get(i).textColor); + assertEquals(-1, options.get(i).backgroundColor); + + i = indexes[3]; + assertNotEquals(-1, i); + assertEquals("", options.get(i).group); + assertEquals("", options.get(i).description); + assertEquals(-1, options.get(i).textColor); + assertEquals(-1, options.get(i).backgroundColor); + + robot.close(); + } + + @Test + void clearOpModes() { + class MyMockRobot extends MockRobot { + MyMockRobot() { + addOpModeFactory(() -> new MockOpMode(), RobotMode.TELEOPERATED, "NoArgOpMode"); + addOpModeFactory(() -> new OneArgOpMode(this), RobotMode.TELEOPERATED, "OneArgOpMode"); + publishOpModes(); + } + } + + MyMockRobot robot = new MyMockRobot(); + + robot.clearOpModes(); + var options = DriverStationSim.getOpModeOptions(); + assertEquals(0, options.length); + + robot.close(); + } + + @Test + void removeOpMode() { + class MyMockRobot extends MockRobot { + MyMockRobot() { + addOpModeFactory(() -> new MockOpMode(), RobotMode.TELEOPERATED, "NoArgOpMode"); + addOpModeFactory(() -> new OneArgOpMode(this), RobotMode.TELEOPERATED, "OneArgOpMode"); + publishOpModes(); + } + } + + MyMockRobot robot = new MyMockRobot(); + + robot.removeOpMode(RobotMode.TELEOPERATED, "NoArgOpMode"); + robot.publishOpModes(); + var options = DriverStationSim.getOpModeOptions(); + assertEquals(1, options.length); + assertEquals("OneArgOpMode", options[0].name); + + robot.close(); + } + + @Test + void nonePeriodic() throws InterruptedException { + class MyMockRobot extends MockRobot { + MyMockRobot() { + addOpModeFactory(() -> new MockOpMode(), RobotMode.TELEOPERATED, "NoArgOpMode"); + addOpModeFactory(() -> new OneArgOpMode(this), RobotMode.TELEOPERATED, "OneArgOpMode"); + publishOpModes(); + } + } + + MyMockRobot robot = new MyMockRobot(); + + Thread robotThread = new Thread(robot::startCompetition); + robotThread.start(); + SimHooks.waitForProgramStart(); + + // Time step to get periodic calls on 20 ms robot loop + SimHooks.stepTiming(0.11); // 110ms + assertEquals(5, robot.m_nonePeriodicCount.get()); + + robot.endCompetition(); + robotThread.join(); + robot.close(); + } + + @Test + void robotPeriodic() throws InterruptedException { + class MyMockRobot extends MockRobot { + MyMockRobot() { + addOpModeFactory(() -> new MockOpMode(), RobotMode.TELEOPERATED, "TestOpMode"); + publishOpModes(); + } + } + + MyMockRobot robot = new MyMockRobot(); + + Thread robotThread = new Thread(robot::startCompetition); + robotThread.start(); + SimHooks.waitForProgramStart(); + + // RobotPeriodic should be called regardless of state + assertEquals(0, robot.m_robotPeriodicCount.get()); + + // Step timing to allow callbacks to execute + SimHooks.stepTiming(kPeriod); + assertEquals(1, robot.m_robotPeriodicCount.get()); + + // Additional time steps should continue calling robotPeriodic + SimHooks.stepTiming(kPeriod); + assertEquals(2, robot.m_robotPeriodicCount.get()); + + robot.endCompetition(); + robotThread.join(); + robot.close(); + } +} diff --git a/wpilibjExamples/src/main/java/org/wpilib/templates/opmode/opmode/MyAuto.java b/wpilibjExamples/src/main/java/org/wpilib/templates/opmode/opmode/MyAuto.java index 15c2d76687..5ae1d4f487 100644 --- a/wpilibjExamples/src/main/java/org/wpilib/templates/opmode/opmode/MyAuto.java +++ b/wpilibjExamples/src/main/java/org/wpilib/templates/opmode/opmode/MyAuto.java @@ -15,13 +15,14 @@ public class MyAuto extends PeriodicOpMode { /** The Robot instance is passed into the opmode via the constructor. */ public MyAuto(Robot robot) { m_robot = robot; - /* - * Can call super(period) to set a different periodic time interval. - * - * Additional periodic methods may be configured with addPeriodic(). - */ } + /* + * This method runs periodically, using the same period as the Robot instance. + * + * Additional periodic methods may be configured with addPeriodic(), + * which can have periods that differ from the main Robot instance. + */ @Override public void periodic() { // Put custom auto code here