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;

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 <units/units.h>
+#include <wpi/deprecated.h>
+
 #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;

wpilibc/src/main/native/include/frc/LinearFilter.h

@@ -10,6 +10,7 @@
 #include <initializer_list>
 #include <vector>
 
+#include <units/units.h>
 #include <wpi/ArrayRef.h>
 
 #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:<br>
@@ -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:<br>

wpilibc/src/main/native/include/frc/Notifier.h

@@ -15,6 +15,8 @@
 #include <utility>
 
 #include <hal/Types.h>
+#include <units/units.h>
+#include <wpi/deprecated.h>
 #include <wpi/mutex.h>
 
 #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.
    *

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 <hal/Types.h>
+#include <units/units.h>
+#include <wpi/deprecated.h>
 
 #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;
 

wpilibc/src/main/native/include/frc/Timer.h

@@ -7,6 +7,7 @@
 
 #pragma once
 
+#include <units/units.h>
 #include <wpi/deprecated.h>
 #include <wpi/mutex.h>
 
@@ -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.
    *

wpilibc/src/main/native/include/frc/controller/PIDController.h

@@ -10,6 +10,8 @@
 #include <functional>
 #include <limits>
 
+#include <units/units.h>
+
 #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.

wpilibc/src/test/native/cpp/LinearFilterNoiseTest.cpp

@@ -11,13 +11,14 @@
 #include <memory>
 #include <random>
 
+#include <units/units.h>
 #include <wpi/math>
 
 #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<double> 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>());
     double noise = distr(gen);
     filterError += std::abs(m_filter->Calculate(theory + noise) - theory);
     noiseGenError += std::abs(noise - theory);

wpilibc/src/test/native/cpp/LinearFilterOutputTest.cpp

@@ -12,13 +12,14 @@
 #include <memory>
 #include <random>
 
+#include <units/units.h>
 #include <wpi/math>
 
 #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<double>()));
   }
 
   RecordProperty("LinearFilterOutput", filterOutput);

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<double>(), 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<double>(),
                    controller->Calculate(.0025, 0));
 }