From f7a93713fa2db70a0fa3410b8c29b4edd65fbccc Mon Sep 17 00:00:00 2001
From: Oblarg <emichaelbarnett@gmail.com>
Date: Wed, 4 Dec 2019 23:40:37 -0500
Subject: [PATCH] Fix up templated TrapezoidProfile classes (#2151)

* Fix two-phase name lookup bug

* Fix param in ProfiledPIDCommand constructor overload

* Fix ProfiledPIDCommand/Controller
---
 .../include/frc2/command/ProfiledPIDCommand.h | 20 +++++++++----------
 .../frc2/command/TrapezoidProfileCommand.h    |  2 +-
 .../frc/controller/ProfiledPIDController.h    |  7 ++++---
 3 files changed, 15 insertions(+), 14 deletions(-)

diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h
index 86cfe0d59e..49575de988 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h
@@ -47,7 +47,7 @@ class ProfiledPIDCommand
    * @param requirements      the subsystems required by this command
    */
   ProfiledPIDCommand(frc::ProfiledPIDController<Distance> controller,
-                     std::function<units::unit_t<Distance>> measurementSource,
+                     std::function<Distance_t()> measurementSource,
                      std::function<State()> goalSource,
                      std::function<void(double, State)> useOutput,
                      std::initializer_list<Subsystem*> requirements = {})
@@ -55,7 +55,7 @@ class ProfiledPIDCommand
         m_measurement{std::move(measurementSource)},
         m_goal{std::move(goalSource)},
         m_useOutput{std::move(useOutput)} {
-    AddRequirements(requirements);
+    this->AddRequirements(requirements);
   }
 
   /**
@@ -69,13 +69,13 @@ class ProfiledPIDCommand
    * @param requirements      the subsystems required by this command
    */
   ProfiledPIDCommand(frc::ProfiledPIDController<Distance> controller,
-                     std::function<units::unit_t<Distance>> measurementSource,
-                     std::function<units::unit_t<Distance>> goalSource,
+                     std::function<Distance_t()> measurementSource,
+                     std::function<Distance_t()> goalSource,
                      std::function<void(double, State)> useOutput,
                      std::initializer_list<Subsystem*> requirements)
       : ProfiledPIDCommand(controller, measurementSource,
                            [&goalSource]() {
-                             return State{goalSource(), 0_mps};
+                             return State{goalSource(), Velocity_t{0}};
                            },
                            useOutput, requirements) {}
 
@@ -90,8 +90,8 @@ class ProfiledPIDCommand
    * @param requirements      the subsystems required by this command
    */
   ProfiledPIDCommand(frc::ProfiledPIDController<Distance> controller,
-                     std::function<units::unit_t<Distance>> measurementSource,
-                     State goal, std::function<void(double, State)> useOutput,
+                     std::function<Distance_t()> measurementSource, State goal,
+                     std::function<void(double, State)> useOutput,
                      std::initializer_list<Subsystem*> requirements)
       : ProfiledPIDCommand(controller, measurementSource,
                            [goal] { return goal; }, useOutput, requirements) {}
@@ -107,8 +107,8 @@ class ProfiledPIDCommand
    * @param requirements      the subsystems required by this command
    */
   ProfiledPIDCommand(frc::ProfiledPIDController<Distance> controller,
-                     std::function<units::unit_t<Distance>> measurementSource,
-                     units::meter_t goal,
+                     std::function<Distance_t()> measurementSource,
+                     Distance_t goal,
                      std::function<void(double, State)> useOutput,
                      std::initializer_list<Subsystem*> requirements)
       : ProfiledPIDCommand(controller, measurementSource,
@@ -138,7 +138,7 @@ class ProfiledPIDCommand
 
  protected:
   frc::ProfiledPIDController<Distance> m_controller;
-  std::function<Distance_t> m_measurement;
+  std::function<Distance_t()> m_measurement;
   std::function<State()> m_goal;
   std::function<void(double, State)> m_useOutput;
 };
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h
index 8d71c7ef73..e21de074dc 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h
@@ -44,7 +44,7 @@ class TrapezoidProfileCommand
                           std::function<void(State)> output,
                           std::initializer_list<Subsystem*> requirements)
       : m_profile(profile), m_output(output) {
-    AddRequirements(requirements);
+    this->AddRequirements(requirements);
   }
 
   void Initialize() override {
diff --git a/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h b/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h
index b6924299ac..96fd331ed9 100644
--- a/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h
+++ b/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h
@@ -217,9 +217,10 @@ class ProfiledPIDController
    * @param velocityTolerance Velocity error which is tolerable.
    */
   void SetTolerance(
-      double positionTolerance,
-      double velocityTolerance = std::numeric_limits<double>::infinity()) {
-    m_controller.SetTolerance(positionTolerance, velocityTolerance);
+      Distance_t positionTolerance,
+      Velocity_t velocityTolerance = std::numeric_limits<double>::infinity()) {
+    m_controller.SetTolerance(positionTolerance.template to<double>(),
+                              velocityTolerance.template to<double>());
   }
 
   /**