mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-20 00:51:42 +00:00
[wpilibc] Transition C++ classes to units::second_t (#3396)
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.
This commit is contained in:
@@ -13,8 +13,8 @@
|
||||
|
||||
using namespace frc;
|
||||
|
||||
static constexpr auto kSamplePeriod = 0.0005_s;
|
||||
static constexpr double kCalibrationSampleTime = 5.0;
|
||||
static constexpr auto kSamplePeriod = 0.5_ms;
|
||||
static constexpr auto kCalibrationSampleTime = 5_s;
|
||||
static constexpr double kDegreePerSecondPerLSB = 0.0125;
|
||||
|
||||
// static constexpr int kRateRegister = 0x00;
|
||||
@@ -121,7 +121,7 @@ void ADXRS450_Gyro::Reset() {
|
||||
}
|
||||
|
||||
void ADXRS450_Gyro::Calibrate() {
|
||||
Wait(0.1);
|
||||
Wait(100_ms);
|
||||
|
||||
m_spi.SetAccumulatorIntegratedCenter(0);
|
||||
m_spi.ResetAccumulator();
|
||||
|
||||
@@ -141,7 +141,7 @@ void AnalogInput::ResetAccumulator() {
|
||||
const double sampleTime = 1.0 / GetSampleRate();
|
||||
const double overSamples = 1 << GetOversampleBits();
|
||||
const double averageSamples = 1 << GetAverageBits();
|
||||
Wait(sampleTime * overSamples * averageSamples);
|
||||
Wait(units::second_t{sampleTime * overSamples * averageSamples});
|
||||
}
|
||||
|
||||
void AnalogInput::SetAccumulatorCenter(int center) {
|
||||
|
||||
@@ -24,7 +24,7 @@ Counter::Counter(Mode mode) {
|
||||
&m_index, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "InitializeCounter");
|
||||
|
||||
SetMaxPeriod(0.5);
|
||||
SetMaxPeriod(0.5_s);
|
||||
|
||||
HAL_Report(HALUsageReporting::kResourceType_Counter, m_index + 1, mode + 1);
|
||||
SendableRegistry::GetInstance().AddLW(this, "Counter", m_index);
|
||||
@@ -273,16 +273,16 @@ void Counter::Reset() {
|
||||
FRC_CheckErrorStatus(status, "{}", "Reset");
|
||||
}
|
||||
|
||||
double Counter::GetPeriod() const {
|
||||
units::second_t Counter::GetPeriod() const {
|
||||
int32_t status = 0;
|
||||
double value = HAL_GetCounterPeriod(m_counter, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "GetPeriod");
|
||||
return value;
|
||||
return units::second_t{value};
|
||||
}
|
||||
|
||||
void Counter::SetMaxPeriod(double maxPeriod) {
|
||||
void Counter::SetMaxPeriod(units::second_t maxPeriod) {
|
||||
int32_t status = 0;
|
||||
HAL_SetCounterMaxPeriod(m_counter, maxPeriod, &status);
|
||||
HAL_SetCounterMaxPeriod(m_counter, maxPeriod.to<double>(), &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "SetMaxPeriod");
|
||||
}
|
||||
|
||||
|
||||
@@ -89,7 +89,7 @@ class MatchDataSender {
|
||||
|
||||
using namespace frc;
|
||||
|
||||
static constexpr double kJoystickUnpluggedMessageInterval = 1.0;
|
||||
static constexpr auto kJoystickUnpluggedMessageInterval = 1_s;
|
||||
|
||||
static int& GetDSLastCount() {
|
||||
// There is a rollover error condition here. At Packet# = n * (uintmax), this
|
||||
@@ -485,12 +485,12 @@ int DriverStation::GetLocation() const {
|
||||
}
|
||||
|
||||
void DriverStation::WaitForData() {
|
||||
WaitForData(0);
|
||||
WaitForData(0_s);
|
||||
}
|
||||
|
||||
bool DriverStation::WaitForData(double timeout) {
|
||||
auto timeoutTime =
|
||||
std::chrono::steady_clock::now() + std::chrono::duration<double>(timeout);
|
||||
bool DriverStation::WaitForData(units::second_t timeout) {
|
||||
auto timeoutTime = std::chrono::steady_clock::now() +
|
||||
std::chrono::steady_clock::duration{timeout};
|
||||
|
||||
std::unique_lock lock(m_waitForDataMutex);
|
||||
int& lastCount = GetDSLastCount();
|
||||
@@ -500,7 +500,7 @@ bool DriverStation::WaitForData(double timeout) {
|
||||
return true;
|
||||
}
|
||||
while (m_waitForDataCounter == currentCount) {
|
||||
if (timeout > 0) {
|
||||
if (timeout > 0_s) {
|
||||
auto timedOut = m_waitForDataCond.wait_until(lock, timeoutTime);
|
||||
if (timedOut == std::cv_status::timeout) {
|
||||
return false;
|
||||
@@ -586,7 +586,7 @@ DriverStation::DriverStation() {
|
||||
|
||||
void DriverStation::ReportJoystickUnpluggedErrorV(fmt::string_view format,
|
||||
fmt::format_args args) {
|
||||
double currentTime = Timer::GetFPGATimestamp();
|
||||
auto currentTime = Timer::GetFPGATimestamp();
|
||||
if (currentTime > m_nextMessageTime) {
|
||||
ReportErrorV(err::Error, "", 0, "", format, args);
|
||||
m_nextMessageTime = currentTime + kJoystickUnpluggedMessageInterval;
|
||||
@@ -596,7 +596,7 @@ void DriverStation::ReportJoystickUnpluggedErrorV(fmt::string_view format,
|
||||
void DriverStation::ReportJoystickUnpluggedWarningV(fmt::string_view format,
|
||||
fmt::format_args args) {
|
||||
if (IsFMSAttached() || !m_silenceJoystickWarning) {
|
||||
double currentTime = Timer::GetFPGATimestamp();
|
||||
auto currentTime = Timer::GetFPGATimestamp();
|
||||
if (currentTime > m_nextMessageTime) {
|
||||
ReportErrorV(warn::Warning, "", 0, "", format, args);
|
||||
m_nextMessageTime = currentTime + kJoystickUnpluggedMessageInterval;
|
||||
|
||||
@@ -79,16 +79,16 @@ void Encoder::Reset() {
|
||||
FRC_CheckErrorStatus(status, "{}", "Reset");
|
||||
}
|
||||
|
||||
double Encoder::GetPeriod() const {
|
||||
units::second_t Encoder::GetPeriod() const {
|
||||
int32_t status = 0;
|
||||
double value = HAL_GetEncoderPeriod(m_encoder, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "GetPeriod");
|
||||
return value;
|
||||
return units::second_t{value};
|
||||
}
|
||||
|
||||
void Encoder::SetMaxPeriod(double maxPeriod) {
|
||||
void Encoder::SetMaxPeriod(units::second_t maxPeriod) {
|
||||
int32_t status = 0;
|
||||
HAL_SetEncoderMaxPeriod(m_encoder, maxPeriod, &status);
|
||||
HAL_SetEncoderMaxPeriod(m_encoder, maxPeriod.to<double>(), &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "SetMaxPeriod");
|
||||
}
|
||||
|
||||
|
||||
@@ -85,12 +85,13 @@ void InterruptableSensorBase::CancelInterrupts() {
|
||||
}
|
||||
|
||||
InterruptableSensorBase::WaitResult InterruptableSensorBase::WaitForInterrupt(
|
||||
double timeout, bool ignorePrevious) {
|
||||
units::second_t timeout, bool ignorePrevious) {
|
||||
FRC_Assert(m_interrupt != HAL_kInvalidHandle);
|
||||
int32_t status = 0;
|
||||
int result;
|
||||
|
||||
result = HAL_WaitForInterrupt(m_interrupt, timeout, ignorePrevious, &status);
|
||||
result = HAL_WaitForInterrupt(m_interrupt, timeout.to<double>(),
|
||||
ignorePrevious, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "WaitForInterrupt");
|
||||
|
||||
// Rising edge result is the interrupt bit set in the byte 0xFF
|
||||
@@ -116,20 +117,20 @@ void InterruptableSensorBase::DisableInterrupts() {
|
||||
FRC_CheckErrorStatus(status, "{}", "DisableInterrupts");
|
||||
}
|
||||
|
||||
double InterruptableSensorBase::ReadRisingTimestamp() {
|
||||
units::second_t InterruptableSensorBase::ReadRisingTimestamp() {
|
||||
FRC_Assert(m_interrupt != HAL_kInvalidHandle);
|
||||
int32_t status = 0;
|
||||
int64_t timestamp = HAL_ReadInterruptRisingTimestamp(m_interrupt, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "ReadRisingTimestamp");
|
||||
return timestamp * 1e-6;
|
||||
return units::microsecond_t(timestamp);
|
||||
}
|
||||
|
||||
double InterruptableSensorBase::ReadFallingTimestamp() {
|
||||
units::second_t InterruptableSensorBase::ReadFallingTimestamp() {
|
||||
FRC_Assert(m_interrupt != HAL_kInvalidHandle);
|
||||
int32_t status = 0;
|
||||
int64_t timestamp = HAL_ReadInterruptFallingTimestamp(m_interrupt, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "ReadFallingTimestamp");
|
||||
return timestamp * 1e-6;
|
||||
return units::microsecond_t(timestamp);
|
||||
}
|
||||
|
||||
void InterruptableSensorBase::SetUpSourceEdge(bool risingEdge,
|
||||
|
||||
@@ -47,12 +47,12 @@ void MotorSafety::Feed() {
|
||||
m_stopTime = Timer::GetFPGATimestamp() + m_expiration;
|
||||
}
|
||||
|
||||
void MotorSafety::SetExpiration(double expirationTime) {
|
||||
void MotorSafety::SetExpiration(units::second_t expirationTime) {
|
||||
std::scoped_lock lock(m_thisMutex);
|
||||
m_expiration = expirationTime;
|
||||
}
|
||||
|
||||
double MotorSafety::GetExpiration() const {
|
||||
units::second_t MotorSafety::GetExpiration() const {
|
||||
std::scoped_lock lock(m_thisMutex);
|
||||
return m_expiration;
|
||||
}
|
||||
@@ -74,7 +74,7 @@ bool MotorSafety::IsSafetyEnabled() const {
|
||||
|
||||
void MotorSafety::Check() {
|
||||
bool enabled;
|
||||
double stopTime;
|
||||
units::second_t stopTime;
|
||||
|
||||
{
|
||||
std::scoped_lock lock(m_thisMutex);
|
||||
|
||||
@@ -151,26 +151,18 @@ void Notifier::SetHandler(std::function<void()> handler) {
|
||||
m_handler = handler;
|
||||
}
|
||||
|
||||
void Notifier::StartSingle(double delay) {
|
||||
StartSingle(units::second_t(delay));
|
||||
}
|
||||
|
||||
void Notifier::StartSingle(units::second_t delay) {
|
||||
std::scoped_lock lock(m_processMutex);
|
||||
m_periodic = false;
|
||||
m_period = delay.to<double>();
|
||||
m_period = delay;
|
||||
m_expirationTime = Timer::GetFPGATimestamp() + m_period;
|
||||
UpdateAlarm();
|
||||
}
|
||||
|
||||
void Notifier::StartPeriodic(double period) {
|
||||
StartPeriodic(units::second_t(period));
|
||||
}
|
||||
|
||||
void Notifier::StartPeriodic(units::second_t period) {
|
||||
std::scoped_lock lock(m_processMutex);
|
||||
m_periodic = true;
|
||||
m_period = period.to<double>();
|
||||
m_period = period;
|
||||
m_expirationTime = Timer::GetFPGATimestamp() + m_period;
|
||||
UpdateAlarm();
|
||||
}
|
||||
|
||||
@@ -307,10 +307,6 @@ int SPI::ReadAutoReceivedData(uint32_t* buffer, int numToRead,
|
||||
return val;
|
||||
}
|
||||
|
||||
int SPI::ReadAutoReceivedData(uint32_t* buffer, int numToRead, double timeout) {
|
||||
return ReadAutoReceivedData(buffer, numToRead, units::second_t(timeout));
|
||||
}
|
||||
|
||||
int SPI::GetAutoDroppedCount() {
|
||||
int32_t status = 0;
|
||||
int32_t val = HAL_GetSPIAutoDroppedCount(m_port, &status);
|
||||
|
||||
@@ -31,7 +31,7 @@ SerialPort::SerialPort(int baudRate, Port port, int dataBits,
|
||||
FRC_CheckErrorStatus(status, "SetSerialStopBits {}", stopBits);
|
||||
|
||||
// Set the default timeout to 5 seconds.
|
||||
SetTimeout(5.0);
|
||||
SetTimeout(5_s);
|
||||
|
||||
// Don't wait until the buffer is full to transmit.
|
||||
SetWriteBufferMode(kFlushOnAccess);
|
||||
@@ -61,7 +61,7 @@ SerialPort::SerialPort(int baudRate, std::string_view portName, Port port,
|
||||
FRC_CheckErrorStatus(status, "SetSerialStopBits {}", stopBits);
|
||||
|
||||
// Set the default timeout to 5 seconds.
|
||||
SetTimeout(5.0);
|
||||
SetTimeout(5_s);
|
||||
|
||||
// Don't wait until the buffer is full to transmit.
|
||||
SetWriteBufferMode(kFlushOnAccess);
|
||||
@@ -122,9 +122,9 @@ int SerialPort::Write(std::string_view buffer) {
|
||||
return retVal;
|
||||
}
|
||||
|
||||
void SerialPort::SetTimeout(double timeout) {
|
||||
void SerialPort::SetTimeout(units::second_t timeout) {
|
||||
int32_t status = 0;
|
||||
HAL_SetSerialTimeout(m_portHandle, timeout, &status);
|
||||
HAL_SetSerialTimeout(m_portHandle, timeout.to<double>(), &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "SetTimeout");
|
||||
}
|
||||
|
||||
|
||||
@@ -76,10 +76,10 @@ bool Solenoid::IsBlackListed() const {
|
||||
return (value != 0);
|
||||
}
|
||||
|
||||
void Solenoid::SetPulseDuration(double durationSeconds) {
|
||||
int32_t durationMS = durationSeconds * 1000;
|
||||
void Solenoid::SetPulseDuration(units::second_t duration) {
|
||||
int32_t status = 0;
|
||||
HAL_SetOneShotDuration(m_solenoidHandle, durationMS, &status);
|
||||
HAL_SetOneShotDuration(m_solenoidHandle,
|
||||
units::millisecond_t{duration}.to<int32_t>(), &status);
|
||||
FRC_CheckErrorStatus(status, "Module {} Channel {}", m_moduleNumber,
|
||||
m_channel);
|
||||
}
|
||||
|
||||
@@ -71,7 +71,7 @@ void TimedRobot::EndCompetition() {
|
||||
TimedRobot::TimedRobot(double period) : TimedRobot(units::second_t(period)) {}
|
||||
|
||||
TimedRobot::TimedRobot(units::second_t period) : IterativeRobotBase(period) {
|
||||
m_startTime = frc2::Timer::GetFPGATimestamp();
|
||||
m_startTime = Timer::GetFPGATimestamp();
|
||||
AddPeriodic([=] { LoopFunc(); }, period);
|
||||
|
||||
int32_t status = 0;
|
||||
|
||||
@@ -4,16 +4,27 @@
|
||||
|
||||
#include "frc/Timer.h"
|
||||
|
||||
#include <units/time.h>
|
||||
#include <chrono>
|
||||
#include <thread>
|
||||
|
||||
#include "frc/DriverStation.h"
|
||||
#include "frc/RobotController.h"
|
||||
|
||||
namespace frc {
|
||||
|
||||
void Wait(double seconds) {
|
||||
frc2::Wait(units::second_t(seconds));
|
||||
void Wait(units::second_t seconds) {
|
||||
std::this_thread::sleep_for(
|
||||
std::chrono::duration<double>(seconds.to<double>()));
|
||||
}
|
||||
|
||||
double GetTime() {
|
||||
return frc2::GetTime().to<double>();
|
||||
units::second_t GetTime() {
|
||||
using std::chrono::duration;
|
||||
using std::chrono::duration_cast;
|
||||
using std::chrono::system_clock;
|
||||
|
||||
return units::second_t(
|
||||
duration_cast<duration<double>>(system_clock::now().time_since_epoch())
|
||||
.count());
|
||||
}
|
||||
|
||||
} // namespace frc
|
||||
@@ -24,30 +35,57 @@ Timer::Timer() {
|
||||
Reset();
|
||||
}
|
||||
|
||||
double Timer::Get() const {
|
||||
return m_timer.Get().to<double>();
|
||||
units::second_t Timer::Get() const {
|
||||
if (m_running) {
|
||||
return (GetFPGATimestamp() - m_startTime) + m_accumulatedTime;
|
||||
} else {
|
||||
return m_accumulatedTime;
|
||||
}
|
||||
}
|
||||
|
||||
void Timer::Reset() {
|
||||
m_timer.Reset();
|
||||
m_accumulatedTime = 0_s;
|
||||
m_startTime = GetFPGATimestamp();
|
||||
}
|
||||
|
||||
void Timer::Start() {
|
||||
m_timer.Start();
|
||||
if (!m_running) {
|
||||
m_startTime = GetFPGATimestamp();
|
||||
m_running = true;
|
||||
}
|
||||
}
|
||||
|
||||
void Timer::Stop() {
|
||||
m_timer.Stop();
|
||||
if (m_running) {
|
||||
m_accumulatedTime = Get();
|
||||
m_running = false;
|
||||
}
|
||||
}
|
||||
|
||||
bool Timer::HasPeriodPassed(double period) {
|
||||
return m_timer.HasPeriodPassed(units::second_t(period));
|
||||
bool Timer::HasElapsed(units::second_t period) const {
|
||||
return Get() > period;
|
||||
}
|
||||
|
||||
double Timer::GetFPGATimestamp() {
|
||||
return frc2::Timer::GetFPGATimestamp().to<double>();
|
||||
bool Timer::HasPeriodPassed(units::second_t period) {
|
||||
return AdvanceIfElapsed(period);
|
||||
}
|
||||
|
||||
double Timer::GetMatchTime() {
|
||||
return frc2::Timer::GetMatchTime().to<double>();
|
||||
bool Timer::AdvanceIfElapsed(units::second_t period) {
|
||||
if (Get() > period) {
|
||||
// Advance the start time by the period.
|
||||
m_startTime += period;
|
||||
// Don't set it to the current time... we want to avoid drift.
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
units::second_t Timer::GetFPGATimestamp() {
|
||||
// FPGA returns the timestamp in microseconds
|
||||
return units::second_t(frc::RobotController::GetFPGATime() * 1.0e-6);
|
||||
}
|
||||
|
||||
units::second_t Timer::GetMatchTime() {
|
||||
return units::second_t(frc::DriverStation::GetInstance().GetMatchTime());
|
||||
}
|
||||
|
||||
@@ -142,14 +142,16 @@ double Ultrasonic::GetRangeInches() const {
|
||||
if (m_simRange) {
|
||||
return m_simRange.Get();
|
||||
}
|
||||
return m_counter.GetPeriod() * kSpeedOfSoundInchesPerSec / 2.0;
|
||||
return units::inch_t{m_counter.GetPeriod() * kSpeedOfSound / 2.0}
|
||||
.to<double>();
|
||||
} else {
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
double Ultrasonic::GetRangeMM() const {
|
||||
return GetRangeInches() * 25.4;
|
||||
return units::millimeter_t{m_counter.GetPeriod() * kSpeedOfSound / 2.0}
|
||||
.to<double>();
|
||||
}
|
||||
|
||||
bool Ultrasonic::IsEnabled() const {
|
||||
@@ -180,7 +182,7 @@ void Ultrasonic::Initialize() {
|
||||
// Link this instance on the list
|
||||
m_sensors.emplace_back(this);
|
||||
|
||||
m_counter.SetMaxPeriod(1.0);
|
||||
m_counter.SetMaxPeriod(1_s);
|
||||
m_counter.SetSemiPeriodMode(true);
|
||||
m_counter.Reset();
|
||||
m_enabled = true; // Make it available for round robin scheduling
|
||||
@@ -204,7 +206,7 @@ void Ultrasonic::UltrasonicChecker() {
|
||||
sensor->m_pingChannel->Pulse(kPingTime); // do the ping
|
||||
}
|
||||
|
||||
Wait(0.1); // wait for ping to return
|
||||
Wait(100_ms); // wait for ping to return
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -14,7 +14,7 @@
|
||||
#include <wpi/priority_queue.h>
|
||||
|
||||
#include "frc/Errors.h"
|
||||
#include "frc2/Timer.h"
|
||||
#include "frc/Timer.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
@@ -131,9 +131,6 @@ void Watchdog::Impl::Main() {
|
||||
}
|
||||
}
|
||||
|
||||
Watchdog::Watchdog(double timeout, std::function<void()> callback)
|
||||
: Watchdog(units::second_t{timeout}, callback) {}
|
||||
|
||||
Watchdog::Watchdog(units::second_t timeout, std::function<void()> callback)
|
||||
: m_timeout(timeout), m_callback(std::move(callback)), m_impl(GetImpl()) {}
|
||||
|
||||
@@ -167,16 +164,12 @@ Watchdog& Watchdog::operator=(Watchdog&& rhs) {
|
||||
return *this;
|
||||
}
|
||||
|
||||
double Watchdog::GetTime() const {
|
||||
return (frc2::Timer::GetFPGATimestamp() - m_startTime).to<double>();
|
||||
}
|
||||
|
||||
void Watchdog::SetTimeout(double timeout) {
|
||||
SetTimeout(units::second_t{timeout});
|
||||
units::second_t Watchdog::GetTime() const {
|
||||
return Timer::GetFPGATimestamp() - m_startTime;
|
||||
}
|
||||
|
||||
void Watchdog::SetTimeout(units::second_t timeout) {
|
||||
m_startTime = frc2::Timer::GetFPGATimestamp();
|
||||
m_startTime = Timer::GetFPGATimestamp();
|
||||
m_tracer.ClearEpochs();
|
||||
|
||||
std::scoped_lock lock(m_impl->m_mutex);
|
||||
@@ -189,9 +182,9 @@ void Watchdog::SetTimeout(units::second_t timeout) {
|
||||
m_impl->UpdateAlarm();
|
||||
}
|
||||
|
||||
double Watchdog::GetTimeout() const {
|
||||
units::second_t Watchdog::GetTimeout() const {
|
||||
std::scoped_lock lock(m_impl->m_mutex);
|
||||
return m_timeout.to<double>();
|
||||
return m_timeout;
|
||||
}
|
||||
|
||||
bool Watchdog::IsExpired() const {
|
||||
@@ -212,7 +205,7 @@ void Watchdog::Reset() {
|
||||
}
|
||||
|
||||
void Watchdog::Enable() {
|
||||
m_startTime = frc2::Timer::GetFPGATimestamp();
|
||||
m_startTime = Timer::GetFPGATimestamp();
|
||||
m_tracer.ClearEpochs();
|
||||
|
||||
std::scoped_lock lock(m_impl->m_mutex);
|
||||
|
||||
@@ -1,133 +0,0 @@
|
||||
// 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 "frc2/Timer.h"
|
||||
|
||||
#include <chrono>
|
||||
#include <thread>
|
||||
|
||||
#include "frc/DriverStation.h"
|
||||
#include "frc/RobotController.h"
|
||||
|
||||
namespace frc2 {
|
||||
|
||||
void Wait(units::second_t seconds) {
|
||||
std::this_thread::sleep_for(
|
||||
std::chrono::duration<double>(seconds.to<double>()));
|
||||
}
|
||||
|
||||
units::second_t GetTime() {
|
||||
using std::chrono::duration;
|
||||
using std::chrono::duration_cast;
|
||||
using std::chrono::system_clock;
|
||||
|
||||
return units::second_t(
|
||||
duration_cast<duration<double>>(system_clock::now().time_since_epoch())
|
||||
.count());
|
||||
}
|
||||
|
||||
} // namespace frc2
|
||||
|
||||
using namespace frc2;
|
||||
|
||||
Timer::Timer() {
|
||||
Reset();
|
||||
}
|
||||
|
||||
Timer::Timer(const Timer& rhs)
|
||||
: m_startTime(rhs.m_startTime),
|
||||
m_accumulatedTime(rhs.m_accumulatedTime),
|
||||
m_running(rhs.m_running) {}
|
||||
|
||||
Timer& Timer::operator=(const Timer& rhs) {
|
||||
std::scoped_lock lock(m_mutex, rhs.m_mutex);
|
||||
|
||||
m_startTime = rhs.m_startTime;
|
||||
m_accumulatedTime = rhs.m_accumulatedTime;
|
||||
m_running = rhs.m_running;
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
Timer::Timer(Timer&& rhs)
|
||||
: m_startTime(std::move(rhs.m_startTime)),
|
||||
m_accumulatedTime(std::move(rhs.m_accumulatedTime)),
|
||||
m_running(std::move(rhs.m_running)) {}
|
||||
|
||||
Timer& Timer::operator=(Timer&& rhs) {
|
||||
std::scoped_lock lock(m_mutex, rhs.m_mutex);
|
||||
|
||||
m_startTime = std::move(rhs.m_startTime);
|
||||
m_accumulatedTime = std::move(rhs.m_accumulatedTime);
|
||||
m_running = std::move(rhs.m_running);
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
units::second_t Timer::Get() const {
|
||||
units::second_t result;
|
||||
units::second_t currentTime = GetFPGATimestamp();
|
||||
|
||||
std::scoped_lock lock(m_mutex);
|
||||
if (m_running) {
|
||||
result = (currentTime - m_startTime) + m_accumulatedTime;
|
||||
} else {
|
||||
result = m_accumulatedTime;
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
void Timer::Reset() {
|
||||
std::scoped_lock lock(m_mutex);
|
||||
m_accumulatedTime = 0_s;
|
||||
m_startTime = GetFPGATimestamp();
|
||||
}
|
||||
|
||||
void Timer::Start() {
|
||||
std::scoped_lock lock(m_mutex);
|
||||
if (!m_running) {
|
||||
m_startTime = GetFPGATimestamp();
|
||||
m_running = true;
|
||||
}
|
||||
}
|
||||
|
||||
void Timer::Stop() {
|
||||
units::second_t temp = Get();
|
||||
|
||||
std::scoped_lock lock(m_mutex);
|
||||
if (m_running) {
|
||||
m_accumulatedTime = temp;
|
||||
m_running = false;
|
||||
}
|
||||
}
|
||||
|
||||
bool Timer::HasElapsed(units::second_t period) const {
|
||||
return Get() > period;
|
||||
}
|
||||
|
||||
bool Timer::HasPeriodPassed(units::second_t period) {
|
||||
return AdvanceIfElapsed(period);
|
||||
}
|
||||
|
||||
bool Timer::AdvanceIfElapsed(units::second_t period) {
|
||||
if (Get() > period) {
|
||||
std::scoped_lock lock(m_mutex);
|
||||
// Advance the start time by the period.
|
||||
m_startTime += period;
|
||||
// Don't set it to the current time... we want to avoid drift.
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
units::second_t Timer::GetFPGATimestamp() {
|
||||
// FPGA returns the timestamp in microseconds
|
||||
return units::second_t(frc::RobotController::GetFPGATime() * 1.0e-6);
|
||||
}
|
||||
|
||||
units::second_t Timer::GetMatchTime() {
|
||||
return units::second_t(frc::DriverStation::GetInstance().GetMatchTime());
|
||||
}
|
||||
@@ -17,7 +17,7 @@ NidecBrushless::NidecBrushless(int pwmChannel, int dioChannel)
|
||||
auto& registry = SendableRegistry::GetInstance();
|
||||
registry.AddChild(this, &m_dio);
|
||||
registry.AddChild(this, &m_pwm);
|
||||
SetExpiration(0.0);
|
||||
SetExpiration(0_s);
|
||||
SetSafetyEnabled(false);
|
||||
|
||||
// the dio controls the output (in PWM mode)
|
||||
|
||||
Reference in New Issue
Block a user