[wpilibc] Move framework sources to framework directory (#8347)

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

@@ -0,0 +1,113 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "wpi/framework/TimedRobot.hpp"
+
+#include <stdint.h>
+
+#include <cstdio>
+#include <utility>
+
+#include "wpi/hal/DriverStation.h"
+#include "wpi/hal/Notifier.h"
+#include "wpi/hal/UsageReporting.h"
+#include "wpi/system/Errors.hpp"
+
+using namespace wpi;
+
+void TimedRobot::StartCompetition() {
+  if constexpr (IsSimulation()) {
+    SimulationInit();
+  }
+
+  // Tell the DS that the robot is ready to be enabled
+  std::puts("\n********** Robot program startup complete **********");
+  HAL_ObserveUserProgramStarting();
+
+  // 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_UpdateNotifierAlarm(m_notifier, callback.expirationTime.count(),
+                            &status);
+    WPILIB_CheckErrorStatus(status, "UpdateNotifierAlarm");
+
+    std::chrono::microseconds currentTime{
+        HAL_WaitForNotifierAlarm(m_notifier, &status)};
+    if (currentTime.count() == 0 || status != 0) {
+      break;
+    }
+
+    m_loopStartTimeUs = RobotController::GetFPGATime();
+
+    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));
+    }
+  }
+}
+
+void TimedRobot::EndCompetition() {
+  int32_t status = 0;
+  HAL_StopNotifier(m_notifier, &status);
+}
+
+TimedRobot::TimedRobot(wpi::units::second_t period)
+    : IterativeRobotBase(period) {
+  m_startTime = std::chrono::microseconds{RobotController::GetFPGATime()};
+  AddPeriodic([=, this] { LoopFunc(); }, period);
+
+  int32_t status = 0;
+  m_notifier = HAL_InitializeNotifier(&status);
+  WPILIB_CheckErrorStatus(status, "InitializeNotifier");
+  HAL_SetNotifierName(m_notifier, "TimedRobot", &status);
+
+  HAL_ReportUsage("Framework", "TimedRobot");
+}
+
+TimedRobot::TimedRobot(wpi::units::hertz_t frequency)
+    : TimedRobot{1 / frequency} {}
+
+TimedRobot::~TimedRobot() {
+  if (m_notifier != HAL_kInvalidHandle) {
+    int32_t status = 0;
+    HAL_StopNotifier(m_notifier, &status);
+    WPILIB_ReportError(status, "StopNotifier");
+  }
+}
+
+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)});
+}