diff --git a/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp b/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp index 93bbd241c1..ea52dc5342 100644 --- a/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp +++ b/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp @@ -27,6 +27,10 @@ IterativeRobotBase::IterativeRobotBase(double period) : m_period(period), m_watchdog(period, [this] { PrintLoopOverrunMessage(); }) {} +IterativeRobotBase::IterativeRobotBase(units::second_t period) + : m_period(period.to()), + m_watchdog(period.to(), [this] { PrintLoopOverrunMessage(); }) {} + void IterativeRobotBase::RobotInit() { wpi::outs() << "Default " << __FUNCTION__ << "() method... Override me!\n"; } diff --git a/wpilibc/src/main/native/cpp/LinearFilter.cpp b/wpilibc/src/main/native/cpp/LinearFilter.cpp index 3438ba0196..95df127cc7 100644 --- a/wpilibc/src/main/native/cpp/LinearFilter.cpp +++ b/wpilibc/src/main/native/cpp/LinearFilter.cpp @@ -25,13 +25,15 @@ LinearFilter::LinearFilter(wpi::ArrayRef ffGains, HAL_Report(HALUsageReporting::kResourceType_LinearFilter, instances); } -LinearFilter LinearFilter::SinglePoleIIR(double timeConstant, double period) { - double gain = std::exp(-period / timeConstant); +LinearFilter LinearFilter::SinglePoleIIR(double timeConstant, + units::second_t period) { + double gain = std::exp(-period.to() / timeConstant); return LinearFilter(1.0 - gain, -gain); } -LinearFilter LinearFilter::HighPass(double timeConstant, double period) { - double gain = std::exp(-period / timeConstant); +LinearFilter LinearFilter::HighPass(double timeConstant, + units::second_t period) { + double gain = std::exp(-period.to() / timeConstant); return LinearFilter({gain, -gain}, {-gain}); } diff --git a/wpilibc/src/main/native/cpp/Notifier.cpp b/wpilibc/src/main/native/cpp/Notifier.cpp index 9b5d3a8282..a2f93f99c6 100644 --- a/wpilibc/src/main/native/cpp/Notifier.cpp +++ b/wpilibc/src/main/native/cpp/Notifier.cpp @@ -96,17 +96,25 @@ void Notifier::SetHandler(std::function 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; + m_period = delay.to(); 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; + m_period = period.to(); m_expirationTime = Timer::GetFPGATimestamp() + m_period; UpdateAlarm(); } diff --git a/wpilibc/src/main/native/cpp/TimedRobot.cpp b/wpilibc/src/main/native/cpp/TimedRobot.cpp index 24ab668bb1..7eed084a6d 100644 --- a/wpilibc/src/main/native/cpp/TimedRobot.cpp +++ b/wpilibc/src/main/native/cpp/TimedRobot.cpp @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -43,7 +43,9 @@ void TimedRobot::StartCompetition() { } } -double TimedRobot::GetPeriod() const { return m_period; } +units::second_t TimedRobot::GetPeriod() const { + return units::second_t(m_period); +} TimedRobot::TimedRobot(double period) : IterativeRobotBase(period) { int32_t status = 0; @@ -54,6 +56,15 @@ TimedRobot::TimedRobot(double period) : IterativeRobotBase(period) { HALUsageReporting::kFramework_Timed); } +TimedRobot::TimedRobot(units::second_t period) : IterativeRobotBase(period) { + int32_t status = 0; + m_notifier = HAL_InitializeNotifier(&status); + wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); + + HAL_Report(HALUsageReporting::kResourceType_Framework, + HALUsageReporting::kFramework_Timed); +} + TimedRobot::~TimedRobot() { int32_t status = 0; diff --git a/wpilibc/src/main/native/cpp/Timer.cpp b/wpilibc/src/main/native/cpp/Timer.cpp index c95802edc1..9eec5327b5 100644 --- a/wpilibc/src/main/native/cpp/Timer.cpp +++ b/wpilibc/src/main/native/cpp/Timer.cpp @@ -100,10 +100,14 @@ void Timer::Stop() { } bool Timer::HasPeriodPassed(double period) { - if (Get() > period) { + return HasPeriodPassed(units::second_t(period)); +} + +bool Timer::HasPeriodPassed(units::second_t period) { + if (Get() > period.to()) { std::scoped_lock lock(m_mutex); // Advance the start time by the period. - m_startTime += period; + m_startTime += period.to(); // Don't set it to the current time... we want to avoid drift. return true; } diff --git a/wpilibc/src/main/native/cpp/controller/PIDController.cpp b/wpilibc/src/main/native/cpp/controller/PIDController.cpp index 2e8d07c2b4..c38d2eda3c 100644 --- a/wpilibc/src/main/native/cpp/controller/PIDController.cpp +++ b/wpilibc/src/main/native/cpp/controller/PIDController.cpp @@ -16,8 +16,13 @@ using namespace frc2; -PIDController::PIDController(double Kp, double Ki, double Kd, double period) - : frc::SendableBase(false), m_Kp(Kp), m_Ki(Ki), m_Kd(Kd), m_period(period) { +PIDController::PIDController(double Kp, double Ki, double Kd, + units::second_t period) + : frc::SendableBase(false), + m_Kp(Kp), + m_Ki(Ki), + m_Kd(Kd), + m_period(period.to()) { static int instances = 0; instances++; HAL_Report(HALUsageReporting::kResourceType_PIDController, instances); @@ -36,7 +41,9 @@ double PIDController::GetI() const { return m_Ki; } double PIDController::GetD() const { return m_Kd; } -double PIDController::GetPeriod() const { return m_period; } +units::second_t PIDController::GetPeriod() const { + return units::second_t(m_period); +} double PIDController::GetOutput() const { return m_output; } @@ -111,7 +118,7 @@ double PIDController::GetError() const { * @return The change in error per second. */ double PIDController::GetDeltaError() const { - return (m_currError - m_prevError) / GetPeriod(); + return (m_currError - m_prevError) / m_period; } double PIDController::Calculate(double measurement, double setpoint) { @@ -158,12 +165,12 @@ double PIDController::Calculate(double measurement) { m_currError = GetContinuousError(m_setpoint - measurement); if (m_Ki != 0) { - m_totalError = std::clamp(m_totalError + m_currError * GetPeriod(), + m_totalError = std::clamp(m_totalError + m_currError * m_period, m_minimumOutput / m_Ki, m_maximumOutput / m_Ki); } m_output = std::clamp(m_Kp * m_currError + m_Ki * m_totalError + - m_Kd * (m_currError - m_prevError) / GetPeriod(), + m_Kd * (m_currError - m_prevError) / m_period, m_minimumOutput, m_maximumOutput); return m_output; diff --git a/wpilibc/src/main/native/include/frc/IterativeRobotBase.h b/wpilibc/src/main/native/include/frc/IterativeRobotBase.h index 6ff5816e1f..6011f48deb 100644 --- a/wpilibc/src/main/native/include/frc/IterativeRobotBase.h +++ b/wpilibc/src/main/native/include/frc/IterativeRobotBase.h @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -7,6 +7,9 @@ #pragma once +#include +#include + #include "frc/RobotBase.h" #include "frc/Watchdog.h" @@ -141,8 +144,16 @@ class IterativeRobotBase : public RobotBase { * * @param period Period in seconds. */ + WPI_DEPRECATED("Use ctor with unit-safety instead.") explicit IterativeRobotBase(double period); + /** + * Constructor for IterativeRobotBase. + * + * @param period Period. + */ + explicit IterativeRobotBase(units::second_t period); + virtual ~IterativeRobotBase() = default; IterativeRobotBase(IterativeRobotBase&&) = default; diff --git a/wpilibc/src/main/native/include/frc/LinearFilter.h b/wpilibc/src/main/native/include/frc/LinearFilter.h index 140f82f160..609c128083 100644 --- a/wpilibc/src/main/native/include/frc/LinearFilter.h +++ b/wpilibc/src/main/native/include/frc/LinearFilter.h @@ -10,6 +10,7 @@ #include #include +#include #include #include "frc/circular_buffer.h" @@ -99,7 +100,8 @@ class LinearFilter { * @param period The period in seconds between samples taken by the * user. */ - static LinearFilter SinglePoleIIR(double timeConstant, double period); + static LinearFilter SinglePoleIIR(double timeConstant, + units::second_t period); /** * Creates a first-order high-pass filter of the form:
@@ -112,7 +114,7 @@ class LinearFilter { * @param period The period in seconds between samples taken by the * user. */ - static LinearFilter HighPass(double timeConstant, double period); + static LinearFilter HighPass(double timeConstant, units::second_t period); /** * Creates a K-tap FIR moving average filter of the form:
diff --git a/wpilibc/src/main/native/include/frc/Notifier.h b/wpilibc/src/main/native/include/frc/Notifier.h index d6c14f3afe..f2f37f1154 100644 --- a/wpilibc/src/main/native/include/frc/Notifier.h +++ b/wpilibc/src/main/native/include/frc/Notifier.h @@ -15,6 +15,8 @@ #include #include +#include +#include #include #include "frc/ErrorBase.h" @@ -58,8 +60,18 @@ class Notifier : public ErrorBase { * * @param delay Seconds to wait before the handler is called. */ + WPI_DEPRECATED("Use unit-safe StartSingle method instead.") void StartSingle(double delay); + /** + * Register for single event notification. + * + * A timer event is queued for a single event after the specified delay. + * + * @param delay Amount of time to wait before the handler is called. + */ + void StartSingle(units::second_t delay); + /** * Register for periodic event notification. * @@ -70,8 +82,21 @@ class Notifier : public ErrorBase { * @param period Period in seconds to call the handler starting one period * after the call to this method. */ + WPI_DEPRECATED("Use unit-safe StartPeriodic method instead.") void StartPeriodic(double period); + /** + * Register for periodic event notification. + * + * A timer event is queued for periodic event notification. Each time the + * interrupt occurs, the event will be immediately requeued for the same time + * interval. + * + * @param period Period to call the handler starting one period + * after the call to this method. + */ + void StartPeriodic(units::second_t period); + /** * Stop timer events from occuring. * diff --git a/wpilibc/src/main/native/include/frc/TimedRobot.h b/wpilibc/src/main/native/include/frc/TimedRobot.h index a6da5ee299..323878fb37 100644 --- a/wpilibc/src/main/native/include/frc/TimedRobot.h +++ b/wpilibc/src/main/native/include/frc/TimedRobot.h @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -8,6 +8,8 @@ #pragma once #include +#include +#include #include "frc/ErrorBase.h" #include "frc/IterativeRobotBase.h" @@ -25,7 +27,7 @@ namespace frc { */ class TimedRobot : public IterativeRobotBase, public ErrorBase { public: - static constexpr double kDefaultPeriod = 0.02; + static constexpr units::second_t kDefaultPeriod = 20_ms; /** * Provide an alternate "main loop" via StartCompetition(). @@ -35,14 +37,22 @@ class TimedRobot : public IterativeRobotBase, public ErrorBase { /** * Get the time period between calls to Periodic() functions. */ - double GetPeriod() const; + units::second_t GetPeriod() const; /** * Constructor for TimedRobot. * * @param period Period in seconds. */ - explicit TimedRobot(double period = kDefaultPeriod); + WPI_DEPRECATED("Use constructor with unit-safety instead.") + explicit TimedRobot(double period); + + /** + * Constructor for TimedRobot. + * + * @param period Period. + */ + explicit TimedRobot(units::second_t period = kDefaultPeriod); ~TimedRobot() override; diff --git a/wpilibc/src/main/native/include/frc/Timer.h b/wpilibc/src/main/native/include/frc/Timer.h index 25f37c8322..c143dcc36c 100644 --- a/wpilibc/src/main/native/include/frc/Timer.h +++ b/wpilibc/src/main/native/include/frc/Timer.h @@ -7,6 +7,7 @@ #pragma once +#include #include #include @@ -101,8 +102,19 @@ class Timer { * @param period The period to check for (in seconds). * @return True if the period has passed. */ + WPI_DEPRECATED("Use unit-safe HasPeriodPassed method instead.") bool HasPeriodPassed(double period); + /** + * Check if the period specified has passed and if it has, advance the start + * time by that period. This is useful to decide if it's time to do periodic + * work without drifting later by the time it took to get around to checking. + * + * @param period The period to check for. + * @return True if the period has passed. + */ + bool HasPeriodPassed(units::second_t period); + /** * Return the FPGA system clock time in seconds. * diff --git a/wpilibc/src/main/native/include/frc/controller/PIDController.h b/wpilibc/src/main/native/include/frc/controller/PIDController.h index 7bffbc01f7..4e57bf57fe 100644 --- a/wpilibc/src/main/native/include/frc/controller/PIDController.h +++ b/wpilibc/src/main/native/include/frc/controller/PIDController.h @@ -10,6 +10,8 @@ #include #include +#include + #include "frc/smartdashboard/SendableBase.h" namespace frc2 { @@ -30,7 +32,8 @@ class PIDController : public frc::SendableBase { * @param period The period between controller updates in seconds. The * default is 0.02 seconds. */ - PIDController(double Kp, double Ki, double Kd, double period = 0.02); + PIDController(double Kp, double Ki, double Kd, + units::second_t period = 20_ms); ~PIDController() override = default; @@ -97,7 +100,7 @@ class PIDController : public frc::SendableBase { * * @return The period of the controller. */ - double GetPeriod() const; + units::second_t GetPeriod() const; /** * Returns the current controller output. diff --git a/wpilibc/src/test/native/cpp/LinearFilterNoiseTest.cpp b/wpilibc/src/test/native/cpp/LinearFilterNoiseTest.cpp index 1363b1cfeb..888d183b69 100644 --- a/wpilibc/src/test/native/cpp/LinearFilterNoiseTest.cpp +++ b/wpilibc/src/test/native/cpp/LinearFilterNoiseTest.cpp @@ -11,13 +11,14 @@ #include #include +#include #include #include "gtest/gtest.h" // Filter constants -static constexpr double kFilterStep = 0.005; -static constexpr double kFilterTime = 2.0; +static constexpr units::second_t kFilterStep = 0.005_s; +static constexpr units::second_t kFilterTime = 2.0_s; static constexpr double kSinglePoleIIRTimeConstant = 0.015915; static constexpr int32_t kMovAvgTaps = 6; @@ -75,8 +76,8 @@ TEST_P(LinearFilterNoiseTest, NoiseReduce) { std::mt19937 gen{rd()}; std::normal_distribution distr{0.0, 10.0}; - for (double t = 0; t < kFilterTime; t += kFilterStep) { - double theory = GetData(t); + for (auto t = 0_s; t < kFilterTime; t += kFilterStep) { + double theory = GetData(t.to()); double noise = distr(gen); filterError += std::abs(m_filter->Calculate(theory + noise) - theory); noiseGenError += std::abs(noise - theory); diff --git a/wpilibc/src/test/native/cpp/LinearFilterOutputTest.cpp b/wpilibc/src/test/native/cpp/LinearFilterOutputTest.cpp index 5e32784421..1f1476c5cb 100644 --- a/wpilibc/src/test/native/cpp/LinearFilterOutputTest.cpp +++ b/wpilibc/src/test/native/cpp/LinearFilterOutputTest.cpp @@ -12,13 +12,14 @@ #include #include +#include #include #include "gtest/gtest.h" // Filter constants -static constexpr double kFilterStep = 0.005; -static constexpr double kFilterTime = 2.0; +static constexpr units::second_t kFilterStep = 0.005_s; +static constexpr units::second_t kFilterTime = 2.0_s; static constexpr double kSinglePoleIIRTimeConstant = 0.015915; static constexpr double kSinglePoleIIRExpectedOutput = -3.2172003; static constexpr double kHighPassTimeConstant = 0.006631; @@ -119,8 +120,8 @@ class LinearFilterOutputTest */ TEST_P(LinearFilterOutputTest, Output) { double filterOutput = 0.0; - for (double t = 0.0; t < kFilterTime; t += kFilterStep) { - filterOutput = m_filter->Calculate(m_data(t)); + for (auto t = 0_s; t < kFilterTime; t += kFilterStep) { + filterOutput = m_filter->Calculate(m_data(t.to())); } RecordProperty("LinearFilterOutput", filterOutput); diff --git a/wpilibc/src/test/native/cpp/PIDInputOutputTest.cpp b/wpilibc/src/test/native/cpp/PIDInputOutputTest.cpp index 73ccfb8293..aad1eca40e 100644 --- a/wpilibc/src/test/native/cpp/PIDInputOutputTest.cpp +++ b/wpilibc/src/test/native/cpp/PIDInputOutputTest.cpp @@ -57,7 +57,7 @@ TEST_F(PIDInputOutputTest, IntegralGainOutputTest) { out = controller->Calculate(.025, 0); } - EXPECT_DOUBLE_EQ(-.5 * controller->GetPeriod(), out); + EXPECT_DOUBLE_EQ(-.5 * controller->GetPeriod().to(), out); } TEST_F(PIDInputOutputTest, DerivativeGainOutputTest) { @@ -65,6 +65,6 @@ TEST_F(PIDInputOutputTest, DerivativeGainOutputTest) { controller->Calculate(0, 0); - EXPECT_DOUBLE_EQ(-.01 / controller->GetPeriod(), + EXPECT_DOUBLE_EQ(-.01 / controller->GetPeriod().to(), controller->Calculate(.0025, 0)); }