mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-19 00:41:43 +00:00
e944ae9aca
- Remove status return from HAL level (clock getting should never fail) - Remove 32-bit timestamp expand function - Make monotonic_clock.hpp (formerly fpga_clock.hpp) header-only and move to root hal include directory
111 lines
3.5 KiB
C++
111 lines
3.5 KiB
C++
// 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.hpp"
|
|
#include "wpi/hal/UsageReporting.hpp"
|
|
#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_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));
|
|
}
|
|
}
|
|
}
|
|
|
|
void TimedRobot::EndCompetition() {
|
|
HAL_DestroyNotifier(m_notifier);
|
|
m_notifier = HAL_kInvalidHandle;
|
|
}
|
|
|
|
TimedRobot::TimedRobot(wpi::units::second_t period)
|
|
: IterativeRobotBase(period) {
|
|
m_startTime = std::chrono::microseconds{RobotController::GetMonotonicTime()};
|
|
AddPeriodic([=, this] { LoopFunc(); }, period);
|
|
|
|
int32_t status = 0;
|
|
m_notifier = HAL_CreateNotifier(&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) {
|
|
HAL_DestroyNotifier(m_notifier);
|
|
}
|
|
}
|
|
|
|
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)});
|
|
}
|