mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-19 00:41:43 +00:00
e09293a15e
A lot of these are breaking changes. frc::Timer was replaced with the contents of frc2::Timer. The others were in-place argument changes or removing deprecated non-unit overloads.
99 lines
2.7 KiB
C++
99 lines
2.7 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 "frc/TimedRobot.h"
|
|
|
|
#include <stdint.h>
|
|
|
|
#include <utility>
|
|
|
|
#include <hal/DriverStation.h>
|
|
#include <hal/FRCUsageReporting.h>
|
|
#include <hal/Notifier.h>
|
|
|
|
#include "frc/Errors.h"
|
|
#include "frc/Timer.h"
|
|
|
|
using namespace frc;
|
|
|
|
void TimedRobot::StartCompetition() {
|
|
RobotInit();
|
|
|
|
if constexpr (IsSimulation()) {
|
|
SimulationInit();
|
|
}
|
|
|
|
// Tell the DS that the robot is ready to be enabled
|
|
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, static_cast<uint64_t>(callback.expirationTime * 1e6),
|
|
&status);
|
|
FRC_CheckErrorStatus(status, "{}", "UpdateNotifierAlarm");
|
|
|
|
uint64_t curTime = HAL_WaitForNotifierAlarm(m_notifier, &status);
|
|
if (curTime == 0 || status != 0) {
|
|
break;
|
|
}
|
|
|
|
callback.func();
|
|
|
|
callback.expirationTime += callback.period;
|
|
m_callbacks.push(std::move(callback));
|
|
|
|
// Process all other callbacks that are ready to run
|
|
while (static_cast<uint64_t>(m_callbacks.top().expirationTime * 1e6) <=
|
|
curTime) {
|
|
callback = m_callbacks.pop();
|
|
|
|
callback.func();
|
|
|
|
callback.expirationTime += callback.period;
|
|
m_callbacks.push(std::move(callback));
|
|
}
|
|
}
|
|
}
|
|
|
|
void TimedRobot::EndCompetition() {
|
|
int32_t status = 0;
|
|
HAL_StopNotifier(m_notifier, &status);
|
|
}
|
|
|
|
TimedRobot::TimedRobot(double period) : TimedRobot(units::second_t(period)) {}
|
|
|
|
TimedRobot::TimedRobot(units::second_t period) : IterativeRobotBase(period) {
|
|
m_startTime = Timer::GetFPGATimestamp();
|
|
AddPeriodic([=] { LoopFunc(); }, period);
|
|
|
|
int32_t status = 0;
|
|
m_notifier = HAL_InitializeNotifier(&status);
|
|
FRC_CheckErrorStatus(status, "{}", "InitializeNotifier");
|
|
HAL_SetNotifierName(m_notifier, "TimedRobot", &status);
|
|
|
|
HAL_Report(HALUsageReporting::kResourceType_Framework,
|
|
HALUsageReporting::kFramework_Timed);
|
|
}
|
|
|
|
TimedRobot::~TimedRobot() {
|
|
int32_t status = 0;
|
|
|
|
HAL_StopNotifier(m_notifier, &status);
|
|
FRC_ReportError(status, "{}", "StopNotifier");
|
|
|
|
HAL_CleanNotifier(m_notifier, &status);
|
|
}
|
|
|
|
void TimedRobot::AddPeriodic(std::function<void()> callback,
|
|
units::second_t period, units::second_t offset) {
|
|
m_callbacks.emplace(callback, m_startTime, period, offset);
|
|
}
|