wpilibc: Add overloads for units (#1815)

Add unit-taking overloads to the following classes:
- IterativeRobotBase
- LinearFilter
- Notifier
- TimedRobot
- Timer (HasPeriodPassed only)
- frc2::PIDController

The corresponding non-units-taking functions have been deprecated.

The return value of TimedRobot::GetPeriod() was updated.
This is a breaking change, users should use to<double> to get the value in seconds.

Other return values, e.g. Timer::Get(), have NOT been updated due to much wider use.

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<double>()),
+      m_watchdog(period.to<double>(), [this] { PrintLoopOverrunMessage(); }) {}
+
 void IterativeRobotBase::RobotInit() {
   wpi::outs() << "Default " << __FUNCTION__ << "() method... Override me!\n";
 }

wpilibc/src/main/native/cpp/LinearFilter.cpp

@@ -25,13 +25,15 @@ LinearFilter::LinearFilter(wpi::ArrayRef<double> 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<double>() / 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<double>() / timeConstant);
   return LinearFilter({gain, -gain}, {-gain});
 }
 

wpilibc/src/main/native/cpp/Notifier.cpp

@@ -96,17 +96,25 @@ void Notifier::SetHandler(std::function<void()> 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<double>();
   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<double>();
   m_expirationTime = Timer::GetFPGATimestamp() + m_period;
   UpdateAlarm();
 }

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;
 

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<double>()) {
     std::scoped_lock lock(m_mutex);
     // Advance the start time by the period.
-    m_startTime += period;
+    m_startTime += period.to<double>();
     // Don't set it to the current time... we want to avoid drift.
     return true;
   }

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<double>()) {
   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;