[wpilib] Change opmodes to purely periodic (#8652)

1. Make the OpMode interface itself periodic; this means the only
differences between `OpMode` and `PeriodicOpMode` are the latter's
methods to add sideloaded periodic callbacks
2. Make OpModeRobot process callbacks in a similar fashion to TimedRobot
and
3. Add some lifecycle functions (discussed below)
4. Pull the callback priority queue from TimedRobot to a new class
called `PeriodicPriorityQueue` so that `TimedRobot` and `OpModeRobot`
have less duplication
5. Fix a typo in the DriverStationJNI class that causes a memory leak
when certain driver station sim calls
6. Port the C++ OpModeRobot tests to Java 

`OpModeRobot` now possesses some `IterativeRobotBase`-stye lifecycle
functions; these functions
1. `robotPeriodic` 
2. `simulationInit` and `simulationPeriodic` 
3. `disabledInit`, `disabledPeriodic`, and `disabledExit`
(note that `simulationInit` and `disabledInit` may be renamed to match
wpilibsuite#8719)

`OpModeRobot` also now processes `OpMode` changes (by the Driver
Station) in its `loopFunc` method, similar to
`IterativeRobotBase.loopFunc` processing game mode changes; `loopFunc`
is, similarly to `TimedRobot`, provided as a default `Callback`

---------

Signed-off-by: Zach Harel <zach@zharel.me>
Co-authored-by: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com>

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

@@ -10,9 +10,10 @@
 #include <utility>
 
 #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<void()> callback,
                              wpi::units::second_t period,
                              wpi::units::second_t offset) {
-  m_callbacks.emplace(
-      callback, m_startTime,
-      std::chrono::microseconds{static_cast<int64_t>(period.value() * 1e6)},
-      std::chrono::microseconds{static_cast<int64_t>(offset.value() * 1e6)});
+  m_callbacks.Add(std::move(callback), m_startTime, period, offset);
 }