mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-19 00:41:43 +00:00
934f8d9c15
It was possible for the alarm to fire between the set alarm and ack, resulting in a hang on next wait. It's not possible to ack before set alarm due to a race in sim step timing, so the fix is to provide an atomic ack and set alarm; the easiest way to implement this in the API was to change ack to optionally also set the alarm again.
120 lines
3.7 KiB
C++
120 lines
3.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 "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();
|
|
|
|
bool first = true;
|
|
|
|
// 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;
|
|
if (first) {
|
|
first = false;
|
|
HAL_SetNotifierAlarm(m_notifier, callback.expirationTime.count(), 0, true,
|
|
&status);
|
|
WPILIB_CheckErrorStatus(status, "SetNotifierAlarm");
|
|
} else {
|
|
HAL_AcknowledgeNotifierAlarm(
|
|
m_notifier, true, callback.expirationTime.count(), 0, true, &status);
|
|
WPILIB_CheckErrorStatus(status, "AcknowledgeNotifierAlarm");
|
|
}
|
|
|
|
if (WPI_WaitForObject(m_notifier) == 0) {
|
|
break;
|
|
}
|
|
|
|
m_loopStartTimeUs = RobotController::GetFPGATime();
|
|
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::GetFPGATime()};
|
|
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)});
|
|
}
|