diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/ProfiledPIDCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/ProfiledPIDCommand.cpp
deleted file mode 100644
index e976702ed9..0000000000
--- a/wpilibNewCommands/src/main/native/cpp/frc2/command/ProfiledPIDCommand.cpp
+++ /dev/null
@@ -1,67 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc2/command/ProfiledPIDCommand.h"
-
-using namespace frc2;
-using State = frc::TrapezoidProfile::State;
-
-ProfiledPIDCommand::ProfiledPIDCommand(
-    frc::ProfiledPIDController controller,
-    std::function<units::meter_t()> measurementSource,
-    std::function<State()> goalSource,
-    std::function<void(double, State)> useOutput,
-    std::initializer_list<Subsystem*> requirements)
-    : m_controller{controller},
-      m_measurement{std::move(measurementSource)},
-      m_goal{std::move(goalSource)},
-      m_useOutput{std::move(useOutput)} {
-  AddRequirements(requirements);
-}
-
-ProfiledPIDCommand::ProfiledPIDCommand(
-    frc::ProfiledPIDController controller,
-    std::function<units::meter_t()> measurementSource,
-    std::function<units::meter_t()> goalSource,
-    std::function<void(double, State)> useOutput,
-    std::initializer_list<Subsystem*> requirements)
-    : ProfiledPIDCommand(controller, measurementSource,
-                         [&goalSource]() {
-                           return State{goalSource(), 0_mps};
-                         },
-                         useOutput, requirements) {}
-
-ProfiledPIDCommand::ProfiledPIDCommand(
-    frc::ProfiledPIDController controller,
-    std::function<units::meter_t()> measurementSource, State goal,
-    std::function<void(double, State)> useOutput,
-    std::initializer_list<Subsystem*> requirements)
-    : ProfiledPIDCommand(controller, measurementSource, [goal] { return goal; },
-                         useOutput, requirements) {}
-
-ProfiledPIDCommand::ProfiledPIDCommand(
-    frc::ProfiledPIDController controller,
-    std::function<units::meter_t()> measurementSource, units::meter_t goal,
-    std::function<void(double, State)> useOutput,
-    std::initializer_list<Subsystem*> requirements)
-    : ProfiledPIDCommand(controller, measurementSource, [goal] { return goal; },
-                         useOutput, requirements) {}
-
-void ProfiledPIDCommand::Initialize() { m_controller.Reset(); }
-
-void ProfiledPIDCommand::Execute() {
-  m_useOutput(m_controller.Calculate(m_measurement(), m_goal()),
-              m_controller.GetSetpoint());
-}
-
-void ProfiledPIDCommand::End(bool interrupted) {
-  m_useOutput(0, State{0_m, 0_mps});
-}
-
-frc::ProfiledPIDController& ProfiledPIDCommand::GetController() {
-  return m_controller;
-}
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/ProfiledPIDSubsystem.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/ProfiledPIDSubsystem.cpp
deleted file mode 100644
index 469e153d8c..0000000000
--- a/wpilibNewCommands/src/main/native/cpp/frc2/command/ProfiledPIDSubsystem.cpp
+++ /dev/null
@@ -1,36 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc2/command/ProfiledPIDSubsystem.h"
-
-using namespace frc2;
-using State = frc::TrapezoidProfile::State;
-
-ProfiledPIDSubsystem::ProfiledPIDSubsystem(
-    frc::ProfiledPIDController controller)
-    : m_controller{controller} {}
-
-void ProfiledPIDSubsystem::Periodic() {
-  if (m_enabled) {
-    UseOutput(m_controller.Calculate(GetMeasurement(), GetGoal()),
-              m_controller.GetSetpoint());
-  }
-}
-
-void ProfiledPIDSubsystem::Enable() {
-  m_controller.Reset();
-  m_enabled = true;
-}
-
-void ProfiledPIDSubsystem::Disable() {
-  UseOutput(0, State{0_m, 0_mps});
-  m_enabled = false;
-}
-
-frc::ProfiledPIDController& ProfiledPIDSubsystem::GetController() {
-  return m_controller;
-}
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/TrapezoidProfileCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/TrapezoidProfileCommand.cpp
deleted file mode 100644
index 08628d2fed..0000000000
--- a/wpilibNewCommands/src/main/native/cpp/frc2/command/TrapezoidProfileCommand.cpp
+++ /dev/null
@@ -1,33 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc2/command/TrapezoidProfileCommand.h"
-
-using namespace frc2;
-
-TrapezoidProfileCommand::TrapezoidProfileCommand(
-    frc::TrapezoidProfile profile,
-    std::function<void(frc::TrapezoidProfile::State)> output,
-    std::initializer_list<Subsystem*> requirements)
-    : m_profile(profile), m_output(output) {
-  AddRequirements(requirements);
-}
-
-void TrapezoidProfileCommand::Initialize() {
-  m_timer.Reset();
-  m_timer.Start();
-}
-
-void TrapezoidProfileCommand::Execute() {
-  m_output(m_profile.Calculate(m_timer.Get()));
-}
-
-void TrapezoidProfileCommand::End(bool interrupted) { m_timer.Stop(); }
-
-bool TrapezoidProfileCommand::IsFinished() {
-  return m_timer.HasPeriodPassed(m_profile.TotalTime());
-}
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h
index bf0c337dab..226fa01bc3 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h
@@ -23,11 +23,16 @@ namespace frc2 {
  * class. The controller calculation and output are performed synchronously in
  * the command's execute() method.
  *
- * @see ProfiledPIDController
+ * @see ProfiledPIDController<Distance>
  */
+template <class Distance>
 class ProfiledPIDCommand
-    : public CommandHelper<CommandBase, ProfiledPIDCommand> {
-  using State = frc::TrapezoidProfile::State;
+    : public CommandHelper<CommandBase, ProfiledPIDCommand<Distance>> {
+  using Distance_t = units::unit_t<Distance>;
+  using Velocity =
+      units::compound_unit<Distance, units::inverse<units::seconds>>;
+  using Velocity_t = units::unit_t<Velocity>;
+  using State = frc::TrapezoidProfile<Distance>::State;
 
  public:
   /**
@@ -40,11 +45,17 @@ class ProfiledPIDCommand
    * @param useOutput         the controller's output
    * @param requirements      the subsystems required by this command
    */
-  ProfiledPIDCommand(frc::ProfiledPIDController controller,
-                     std::function<units::meter_t()> measurementSource,
+  ProfiledPIDCommand(frc::ProfiledPIDController<Distance> controller,
+                     std::function<units::unit_t<Distance>> measurementSource,
                      std::function<State()> goalSource,
                      std::function<void(double, State)> useOutput,
-                     std::initializer_list<Subsystem*> requirements = {});
+                     std::initializer_list<Subsystem*> requirements = {})
+                     : m_controller{controller},
+                           m_measurement{std::move(measurementSource)},
+                           m_goal{std::move(goalSource)},
+                           m_useOutput{std::move(useOutput)} {
+    AddRequirements(requirements);
+  }
 
   /**
    * Creates a new PIDCommand, which controls the given output with a
@@ -56,11 +67,16 @@ class ProfiledPIDCommand
    * @param useOutput         the controller's output
    * @param requirements      the subsystems required by this command
    */
-  ProfiledPIDCommand(frc::ProfiledPIDController controller,
-                     std::function<units::meter_t()> measurementSource,
-                     std::function<units::meter_t()> goalSource,
+  ProfiledPIDCommand(frc::ProfiledPIDController<Distance> controller,
+                     std::function<units::unit_t<Distance>> measurementSource,
+                     std::function<units::unit_t<Distance>> goalSource,
                      std::function<void(double, State)> useOutput,
-                     std::initializer_list<Subsystem*> requirements);
+                     std::initializer_list<Subsystem*> requirements)
+                     : ProfiledPIDCommand(controller, measurementSource,
+                                          [&goalSource]() {
+                                            return State{goalSource(), 0_mps};
+                                          },
+                                          useOutput, requirements) {}
 
   /**
    * Creates a new PIDCommand, which controls the given output with a
@@ -72,10 +88,12 @@ class ProfiledPIDCommand
    * @param useOutput         the controller's output
    * @param requirements      the subsystems required by this command
    */
-  ProfiledPIDCommand(frc::ProfiledPIDController controller,
-                     std::function<units::meter_t()> measurementSource,
+  ProfiledPIDCommand(frc::ProfiledPIDController<Distance> controller,
+                     std::function<units::unit_t<Distance>> measurementSource,
                      State goal, std::function<void(double, State)> useOutput,
-                     std::initializer_list<Subsystem*> requirements);
+                     std::initializer_list<Subsystem*> requirements)
+                     : ProfiledPIDCommand(controller, measurementSource, [goal] { return goal; },
+                                          useOutput, requirements) {}
 
   /**
    * Creates a new PIDCommand, which controls the given output with a
@@ -87,32 +105,41 @@ class ProfiledPIDCommand
    * @param useOutput         the controller's output
    * @param requirements      the subsystems required by this command
    */
-  ProfiledPIDCommand(frc::ProfiledPIDController controller,
-                     std::function<units::meter_t()> measurementSource,
+  ProfiledPIDCommand(frc::ProfiledPIDController<Distance> controller,
+                     std::function<units::unit_t<Distance>> measurementSource,
                      units::meter_t goal,
                      std::function<void(double, State)> useOutput,
-                     std::initializer_list<Subsystem*> requirements);
+                     std::initializer_list<Subsystem*> requirements)
+                     : ProfiledPIDCommand(controller, measurementSource, [goal] { return goal; },
+                                          useOutput, requirements) {}
 
   ProfiledPIDCommand(ProfiledPIDCommand&& other) = default;
 
   ProfiledPIDCommand(const ProfiledPIDCommand& other) = default;
 
-  void Initialize() override;
+  void Initialize() override { m_controller.Reset(); }
 
-  void Execute() override;
+  void Execute() override {
+    m_useOutput(m_controller.Calculate(m_measurement(), m_goal()),
+                m_controller.GetSetpoint());
+  }
 
-  void End(bool interrupted) override;
+  void End(bool interrupted) override {
+    m_useOutput(0, State{Distance_t(0), Velocity_t(0)});
+  }
 
   /**
    * Returns the ProfiledPIDController used by the command.
    *
    * @return The ProfiledPIDController
    */
-  frc::ProfiledPIDController& GetController();
+  frc::ProfiledPIDController<Distance>& GetController() {
+    return m_controller;
+  }
 
  protected:
-  frc::ProfiledPIDController m_controller;
-  std::function<units::meter_t()> m_measurement;
+  frc::ProfiledPIDController<Distance> m_controller;
+  std::function<units::unit_t<Distance>> m_measurement;
   std::function<State()> m_goal;
   std::function<void(double, State)> m_useOutput;
 };
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h b/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h
index 02497dee01..41815c6162 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h
@@ -19,8 +19,13 @@ namespace frc2 {
  *
  * @see ProfiledPIDController
  */
+template <class Distance>
 class ProfiledPIDSubsystem : public SubsystemBase {
-  using State = frc::TrapezoidProfile::State;
+  using Distance_t = units::unit_t<Distance>;
+  using Velocity =
+      units::compound_unit<Distance, units::inverse<units::seconds>>;
+  using Velocity_t = units::unit_t<Velocity>;
+  using State = frc::TrapezoidProfile<Distance>::State;
 
  public:
   /**
@@ -28,9 +33,15 @@ class ProfiledPIDSubsystem : public SubsystemBase {
    *
    * @param controller the ProfiledPIDController to use
    */
-  explicit ProfiledPIDSubsystem(frc::ProfiledPIDController controller);
+  explicit ProfiledPIDSubsystem(frc::ProfiledPIDController<Distance> controller)
+                                : m_controller{controller} {}
 
-  void Periodic() override;
+  void Periodic() override {
+    if (m_enabled) {
+      UseOutput(m_controller.Calculate(GetMeasurement(), GetGoal()),
+                m_controller.GetSetpoint());
+    }
+  }
 
   /**
    * Uses the output from the ProfiledPIDController.
@@ -59,22 +70,30 @@ class ProfiledPIDSubsystem : public SubsystemBase {
   /**
    * Enables the PID control.  Resets the controller.
    */
-  virtual void Enable();
+  virtual void Enable() {
+    m_controller.Reset();
+    m_enabled = true;
+  }
 
   /**
    * Disables the PID control.  Sets output to zero.
    */
-  virtual void Disable();
+  virtual void Disable() {
+     UseOutput(0, State{Distance_t(0), Velocity_t(0)});
+     m_enabled = false;
+   }
 
   /**
    * Returns the ProfiledPIDController.
    *
    * @return The controller.
    */
-  frc::ProfiledPIDController& GetController();
+  frc::ProfiledPIDController<Distance>& GetController() {
+    return m_controller;
+  }
 
  protected:
-  frc::ProfiledPIDController m_controller;
+  frc::ProfiledPIDController<Distance> m_controller;
   bool m_enabled;
 };
 }  // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h
index 90ec8b5de3..afa46b1d4f 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h
@@ -23,8 +23,14 @@ namespace frc2 {
  *
  * @see TrapezoidProfile
  */
+template <class Distance>
 class TrapezoidProfileCommand
-    : public CommandHelper<CommandBase, TrapezoidProfileCommand> {
+    : public CommandHelper<CommandBase, TrapezoidProfileCommand<Distance>> {
+  using Distance_t = units::unit_t<Distance>;
+  using Velocity =
+      units::compound_unit<Distance, units::inverse<units::seconds>>;
+  using Velocity_t = units::unit_t<Velocity>;
+  using State = frc::TrapezoidProfile<Distance>::State;
  public:
   /**
    * Creates a new TrapezoidProfileCommand that will execute the given
@@ -34,21 +40,31 @@ class TrapezoidProfileCommand
    * @param output  The consumer for the profile output.
    */
   TrapezoidProfileCommand(
-      frc::TrapezoidProfile profile,
-      std::function<void(frc::TrapezoidProfile::State)> output,
-      std::initializer_list<Subsystem*> requirements);
+      frc::TrapezoidProfile<Distance> profile,
+      std::function<void(State)> output,
+      std::initializer_list<Subsystem*> requirements)
+      : m_profile(profile), m_output(output) {
+    AddRequirements(requirements);
+  }
 
-  void Initialize() override;
+  void Initialize() override {
+    m_timer.Reset();
+    m_timer.Start();
+  }
 
-  void Execute() override;
+  void Execute() override {
+    m_output(m_profile.Calculate(m_timer.Get()));
+  }
 
-  void End(bool interrupted) override;
+  void End(bool interrupted) override { m_timer.Stop(); }
 
-  bool IsFinished() override;
+  bool IsFinished() override {
+    return m_timer.HasPeriodPassed(m_profile.TotalTime());
+  }
 
  private:
-  frc::TrapezoidProfile m_profile;
-  std::function<void(frc::TrapezoidProfile::State)> m_output;
+  frc::TrapezoidProfile<Distance> m_profile;
+  std::function<void(State)> m_output;
 
   Timer m_timer;
 };
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileSubsystem.h b/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileSubsystem.h
index f9206c5dc1..66c6bc3a87 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileSubsystem.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileSubsystem.h
@@ -17,10 +17,14 @@ namespace frc2 {
  * A subsystem that generates and runs trapezoidal motion profiles automatically.  The user
  * specifies how to use the current state of the motion profile by overriding the `UseState` method.
  */
-template <class Unit>
+template <class Distance>
 class TrapezoidProfileSubsystem : public SubsystemBase {
-  using State = frc::TrapezoidProfile::State;
-  using Constraints = frc::TrapezoidProfile::Constraints;
+  using Distance_t = units::unit_t<Distance>;
+  using Velocity =
+      units::compound_unit<Distance, units::inverse<units::seconds>>;
+  using Velocity_t = units::unit_t<Velocity>;
+  using State = frc::TrapezoidProfile<Distance>::State;
+  using Constraints = frc::TrapezoidProfile<Distance>::Constraints;
  public:
   /**
    * Creates a new TrapezoidProfileSubsystem.
@@ -31,14 +35,14 @@ class TrapezoidProfileSubsystem : public SubsystemBase {
    * @param period          The period of the main robot loop, in seconds.
    */
   TrapezoidProfileSubsystem(Constraints constraints,
-                            units::unit_t<Unit> position,
+                            Distance_t position,
                             units::second_t period = 20_ms)
                             : m_constraints(constraints),
-                              m_state{units::meter_t(position.template to<double>()), 0_mps},
+                              m_state{position, Velocity_t(0)},
                               m_period(period) {}
 
   void Periodic() override {
-    auto profile = frc::TrapezoidProfile(m_constraints, GetGoal(), m_state);
+    auto profile = frc::TrapezoidProfile<Distance>(m_constraints, GetGoal(), m_state);
     m_state = profile.Calculate(m_period);
     UseState(m_state);
   }
diff --git a/wpilibc/src/main/native/cpp/controller/ProfiledPIDController.cpp b/wpilibc/src/main/native/cpp/controller/ProfiledPIDController.cpp
deleted file mode 100644
index 4ef3bf9a4d..0000000000
--- a/wpilibc/src/main/native/cpp/controller/ProfiledPIDController.cpp
+++ /dev/null
@@ -1,133 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/controller/ProfiledPIDController.h"
-
-#include <algorithm>
-#include <cmath>
-
-#include "frc/smartdashboard/SendableBuilder.h"
-
-using namespace frc;
-
-ProfiledPIDController::ProfiledPIDController(
-    double Kp, double Ki, double Kd,
-    frc::TrapezoidProfile::Constraints constraints, units::second_t period)
-    : m_controller(Kp, Ki, Kd, period), m_constraints(constraints) {}
-
-void ProfiledPIDController::SetP(double Kp) { m_controller.SetP(Kp); }
-
-void ProfiledPIDController::SetI(double Ki) { m_controller.SetI(Ki); }
-
-void ProfiledPIDController::SetD(double Kd) { m_controller.SetD(Kd); }
-
-double ProfiledPIDController::GetP() const { return m_controller.GetP(); }
-
-double ProfiledPIDController::GetI() const { return m_controller.GetI(); }
-
-double ProfiledPIDController::GetD() const { return m_controller.GetD(); }
-
-units::second_t ProfiledPIDController::GetPeriod() const {
-  return m_controller.GetPeriod();
-}
-
-void ProfiledPIDController::SetGoal(TrapezoidProfile::State goal) {
-  m_goal = goal;
-}
-
-void ProfiledPIDController::SetGoal(units::meter_t goal) {
-  m_goal = {goal, 0_mps};
-}
-
-TrapezoidProfile::State ProfiledPIDController::GetGoal() const {
-  return m_goal;
-}
-
-bool ProfiledPIDController::AtGoal() const {
-  return AtSetpoint() && m_goal == m_setpoint;
-}
-
-void ProfiledPIDController::SetConstraints(
-    frc::TrapezoidProfile::Constraints constraints) {
-  m_constraints = constraints;
-}
-
-TrapezoidProfile::State ProfiledPIDController::GetSetpoint() const {
-  return m_setpoint;
-}
-
-bool ProfiledPIDController::AtSetpoint() const {
-  return m_controller.AtSetpoint();
-}
-
-void ProfiledPIDController::EnableContinuousInput(double minimumInput,
-                                                  double maximumInput) {
-  m_controller.EnableContinuousInput(minimumInput, maximumInput);
-}
-
-void ProfiledPIDController::DisableContinuousInput() {
-  m_controller.DisableContinuousInput();
-}
-
-void ProfiledPIDController::SetIntegratorRange(double minimumIntegral,
-                                               double maximumIntegral) {
-  m_controller.SetIntegratorRange(minimumIntegral, maximumIntegral);
-}
-
-void ProfiledPIDController::SetTolerance(double positionTolerance,
-                                         double velocityTolerance) {
-  m_controller.SetTolerance(positionTolerance, velocityTolerance);
-}
-
-double ProfiledPIDController::GetPositionError() const {
-  return m_controller.GetPositionError();
-}
-
-double ProfiledPIDController::GetVelocityError() const {
-  return m_controller.GetVelocityError();
-}
-
-double ProfiledPIDController::Calculate(units::meter_t measurement) {
-  frc::TrapezoidProfile profile{m_constraints, m_goal, m_setpoint};
-  m_setpoint = profile.Calculate(GetPeriod());
-  return m_controller.Calculate(measurement.to<double>(),
-                                m_setpoint.position.to<double>());
-}
-
-double ProfiledPIDController::Calculate(units::meter_t measurement,
-                                        TrapezoidProfile::State goal) {
-  SetGoal(goal);
-  return Calculate(measurement);
-}
-
-double ProfiledPIDController::Calculate(units::meter_t measurement,
-                                        units::meter_t goal) {
-  SetGoal(goal);
-  return Calculate(measurement);
-}
-
-double ProfiledPIDController::Calculate(
-    units::meter_t measurement, units::meter_t goal,
-    frc::TrapezoidProfile::Constraints constraints) {
-  SetConstraints(constraints);
-  return Calculate(measurement, goal);
-}
-
-void ProfiledPIDController::Reset() { m_controller.Reset(); }
-
-void ProfiledPIDController::InitSendable(frc::SendableBuilder& builder) {
-  builder.SetSmartDashboardType("ProfiledPIDController");
-  builder.AddDoubleProperty("p", [this] { return GetP(); },
-                            [this](double value) { SetP(value); });
-  builder.AddDoubleProperty("i", [this] { return GetI(); },
-                            [this](double value) { SetI(value); });
-  builder.AddDoubleProperty("d", [this] { return GetD(); },
-                            [this](double value) { SetD(value); });
-  builder.AddDoubleProperty(
-      "goal", [this] { return GetGoal().position.to<double>(); },
-      [this](double value) { SetGoal(units::meter_t{value}); });
-}
diff --git a/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h b/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h
index 64da9ebbad..b6924299ac 100644
--- a/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h
+++ b/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h
@@ -7,6 +7,8 @@
 
 #pragma once
 
+#include <algorithm>
+#include <cmath>
 #include <functional>
 #include <limits>
 
@@ -14,6 +16,7 @@
 
 #include "frc/controller/PIDController.h"
 #include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableBuilder.h"
 #include "frc/smartdashboard/SendableHelper.h"
 #include "frc/trajectory/TrapezoidProfile.h"
 
@@ -23,8 +26,20 @@ namespace frc {
  * Implements a PID control loop whose setpoint is constrained by a trapezoid
  * profile.
  */
-class ProfiledPIDController : public Sendable,
-                              public SendableHelper<ProfiledPIDController> {
+template <class Distance>
+class ProfiledPIDController
+    : public Sendable,
+      public SendableHelper<ProfiledPIDController<Distance>> {
+  using Distance_t = units::unit_t<Distance>;
+  using Velocity =
+      units::compound_unit<Distance, units::inverse<units::seconds>>;
+  using Velocity_t = units::unit_t<Velocity>;
+  using Acceleration =
+      units::compound_unit<Velocity, units::inverse<units::seconds>>;
+  using Acceleration_t = units::unit_t<Acceleration>;
+  using State = typename TrapezoidProfile<Distance>::State;
+  using Constraints = typename TrapezoidProfile<Distance>::Constraints;
+
  public:
   /**
    * Allocates a ProfiledPIDController with the given constants for Kp, Ki, and
@@ -38,8 +53,8 @@ class ProfiledPIDController : public Sendable,
    *                    default is 20 milliseconds.
    */
   ProfiledPIDController(double Kp, double Ki, double Kd,
-                        frc::TrapezoidProfile::Constraints constraints,
-                        units::second_t period = 20_ms);
+                        Constraints constraints, units::second_t period = 20_ms)
+      : m_controller(Kp, Ki, Kd, period), m_constraints(constraints) {}
 
   ~ProfiledPIDController() override = default;
 
@@ -57,96 +72,98 @@ class ProfiledPIDController : public Sendable,
    * @param Ki Integral coefficient
    * @param Kd Differential coefficient
    */
-  void SetPID(double Kp, double Ki, double Kd);
+  void SetPID(double Kp, double Ki, double Kd) {
+    m_controller.SetPID(Kp, Ki, Kd);
+  }
 
   /**
    * Sets the proportional coefficient of the PID controller gain.
    *
    * @param Kp proportional coefficient
    */
-  void SetP(double Kp);
+  void SetP(double Kp) { m_controller.SetP(Kp); }
 
   /**
    * Sets the integral coefficient of the PID controller gain.
    *
    * @param Ki integral coefficient
    */
-  void SetI(double Ki);
+  void SetI(double Ki) { m_controller.SetI(Ki); }
 
   /**
    * Sets the differential coefficient of the PID controller gain.
    *
    * @param Kd differential coefficient
    */
-  void SetD(double Kd);
+  void SetD(double Kd) { m_controller.SetD(Kd); }
 
   /**
    * Gets the proportional coefficient.
    *
    * @return proportional coefficient
    */
-  double GetP() const;
+  double GetP() const { return m_controller.GetP(); }
 
   /**
    * Gets the integral coefficient.
    *
    * @return integral coefficient
    */
-  double GetI() const;
+  double GetI() const { return m_controller.GetI(); }
 
   /**
    * Gets the differential coefficient.
    *
    * @return differential coefficient
    */
-  double GetD() const;
+  double GetD() const { return m_controller.GetD(); }
 
   /**
    * Gets the period of this controller.
    *
    * @return The period of the controller.
    */
-  units::second_t GetPeriod() const;
+  units::second_t GetPeriod() const { return m_controller.GetPeriod(); }
 
   /**
    * Sets the goal for the ProfiledPIDController.
    *
    * @param goal The desired unprofiled setpoint.
    */
-  void SetGoal(TrapezoidProfile::State goal);
+  void SetGoal(State goal) { m_goal = goal; }
 
   /**
    * Sets the goal for the ProfiledPIDController.
    *
    * @param goal The desired unprofiled setpoint.
    */
-  void SetGoal(units::meter_t goal);
+  void SetGoal(Distance_t goal) { m_goal = {goal, Velocity_t(0)}; }
 
   /**
    * Gets the goal for the ProfiledPIDController.
    */
-  TrapezoidProfile::State GetGoal() const;
+  State GetGoal() const { return m_goal; }
 
   /**
    * Returns true if the error is within the tolerance of the error.
    *
    * This will return false until at least one input value has been computed.
    */
-  bool AtGoal() const;
+  bool AtGoal() const { return AtSetpoint() && m_goal == m_setpoint; }
 
   /**
    * Set velocity and acceleration constraints for goal.
    *
    * @param constraints Velocity and acceleration constraints for goal.
    */
-  void SetConstraints(frc::TrapezoidProfile::Constraints constraints);
+  void SetConstraints(Constraints constraints) { m_constraints = constraints; }
 
   /**
    * Returns the current setpoint of the ProfiledPIDController.
    *
    * @return The current setpoint.
    */
-  TrapezoidProfile::State GetSetpoint() const;
+  State GetSetpoint() const { return m_setpoint; }
 
   /**
    * Returns true if the error is within the tolerance of the error.
@@ -157,7 +174,7 @@ class ProfiledPIDController : public Sendable,
    *
    * This will return false until at least one input value has been computed.
    */
-  bool AtSetpoint() const;
+  bool AtSetpoint() const { return m_controller.AtSetpoint(); }
 
   /**
    * Enables continuous input.
@@ -169,12 +186,15 @@ class ProfiledPIDController : public Sendable,
    * @param minimumInput The minimum value expected from the input.
    * @param maximumInput The maximum value expected from the input.
    */
-  void EnableContinuousInput(double minimumInput, double maximumInput);
+  void EnableContinuousInput(Distance_t minimumInput, Distance_t maximumInput) {
+    m_controller.EnableContinuousInput(minimumInput.template to<double>(),
+                                       maximumInput.template to<double>());
+  }
 
   /**
    * Disables continuous input.
    */
-  void DisableContinuousInput();
+  void DisableContinuousInput() { m_controller.DisableContinuousInput(); }
 
   /**
    * Sets the minimum and maximum values for the integrator.
@@ -185,7 +205,9 @@ class ProfiledPIDController : public Sendable,
    * @param minimumIntegral The minimum value of the integrator.
    * @param maximumIntegral The maximum value of the integrator.
    */
-  void SetIntegratorRange(double minimumIntegral, double maximumIntegral);
+  void SetIntegratorRange(double minimumIntegral, double maximumIntegral) {
+    m_controller.SetIntegratorRange(minimumIntegral, maximumIntegral);
+  }
 
   /**
    * Sets the error which is considered tolerable for use with
@@ -196,26 +218,37 @@ class ProfiledPIDController : public Sendable,
    */
   void SetTolerance(
       double positionTolerance,
-      double velocityTolerance = std::numeric_limits<double>::infinity());
+      double velocityTolerance = std::numeric_limits<double>::infinity()) {
+    m_controller.SetTolerance(positionTolerance, velocityTolerance);
+  }
 
   /**
    * Returns the difference between the setpoint and the measurement.
    *
    * @return The error.
    */
-  double GetPositionError() const;
+  Distance_t GetPositionError() const {
+    return Distance_t(m_controller.GetPositionError());
+  }
 
   /**
    * Returns the change in error per second.
    */
-  double GetVelocityError() const;
+  Velocity_t GetVelocityError() const {
+    return Velocity_t(m_controller.GetVelocityError());
+  }
 
   /**
    * Returns the next output of the PID controller.
    *
    * @param measurement The current measurement of the process variable.
    */
-  double Calculate(units::meter_t measurement);
+  double Calculate(Distance_t measurement) {
+    frc::TrapezoidProfile<Distance> profile{m_constraints, m_goal, m_setpoint};
+    m_setpoint = profile.Calculate(GetPeriod());
+    return m_controller.Calculate(measurement.template to<double>(),
+                                  m_setpoint.position.template to<double>());
+  }
 
   /**
    * Returns the next output of the PID controller.
@@ -223,15 +256,20 @@ class ProfiledPIDController : public Sendable,
    * @param measurement The current measurement of the process variable.
    * @param goal The new goal of the controller.
    */
-  double Calculate(units::meter_t measurement, TrapezoidProfile::State goal);
-
+  double Calculate(Distance_t measurement, State goal) {
+    SetGoal(goal);
+    return Calculate(measurement);
+  }
   /**
    * Returns the next output of the PID controller.
    *
    * @param measurement The current measurement of the process variable.
    * @param goal The new goal of the controller.
    */
-  double Calculate(units::meter_t measurement, units::meter_t goal);
+  double Calculate(Distance_t measurement, Distance_t goal) {
+    SetGoal(goal);
+    return Calculate(measurement);
+  }
 
   /**
    * Returns the next output of the PID controller.
@@ -240,21 +278,36 @@ class ProfiledPIDController : public Sendable,
    * @param goal        The new goal of the controller.
    * @param constraints Velocity and acceleration constraints for goal.
    */
-  double Calculate(units::meter_t measurement, units::meter_t goal,
-                   frc::TrapezoidProfile::Constraints constraints);
+  double Calculate(
+      Distance_t measurement, Distance_t goal,
+      typename frc::TrapezoidProfile<Distance>::Constraints constraints) {
+    SetConstraints(constraints);
+    return Calculate(measurement, goal);
+  }
 
   /**
    * Reset the previous error, the integral term, and disable the controller.
    */
-  void Reset();
+  void Reset() { m_controller.Reset(); }
 
-  void InitSendable(frc::SendableBuilder& builder) override;
+  void InitSendable(frc::SendableBuilder& builder) override {
+    builder.SetSmartDashboardType("ProfiledPIDController");
+    builder.AddDoubleProperty("p", [this] { return GetP(); },
+                              [this](double value) { SetP(value); });
+    builder.AddDoubleProperty("i", [this] { return GetI(); },
+                              [this](double value) { SetI(value); });
+    builder.AddDoubleProperty("d", [this] { return GetD(); },
+                              [this](double value) { SetD(value); });
+    builder.AddDoubleProperty(
+        "goal", [this] { return GetGoal().position.template to<double>(); },
+        [this](double value) { SetGoal(Distance_t{value}); });
+  }
 
  private:
   frc2::PIDController m_controller;
-  frc::TrapezoidProfile::State m_goal;
-  frc::TrapezoidProfile::State m_setpoint;
-  frc::TrapezoidProfile::Constraints m_constraints;
+  typename frc::TrapezoidProfile<Distance>::State m_goal;
+  typename frc::TrapezoidProfile<Distance>::State m_setpoint;
+  typename frc::TrapezoidProfile<Distance>::Constraints m_constraints;
 };
 
 }  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.h b/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.h
index c528c9e7fd..0479cfec4c 100644
--- a/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.h
+++ b/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.h
@@ -40,18 +40,27 @@ namespace frc {
  * `Calculate()` and to determine when the profile has completed via
  * `IsFinished()`.
  */
+template <class Distance>
 class TrapezoidProfile {
+  using Distance_t = units::unit_t<Distance>;
+  using Velocity =
+      units::compound_unit<Distance, units::inverse<units::seconds>>;
+  using Velocity_t = units::unit_t<Velocity>;
+  using Acceleration =
+      units::compound_unit<Velocity, units::inverse<units::seconds>>;
+  using Acceleration_t = units::unit_t<Acceleration>;
+
  public:
   class Constraints {
    public:
-    units::meters_per_second_t maxVelocity = 0_mps;
-    units::meters_per_second_squared_t maxAcceleration = 0_mps_sq;
+    Velocity_t maxVelocity{0};
+    Acceleration_t maxAcceleration{0};
   };
 
   class State {
    public:
-    units::meter_t position = 0_m;
-    units::meters_per_second_t velocity = 0_mps;
+    Distance_t position{0};
+    Velocity_t velocity{0};
     bool operator==(const State& rhs) const {
       return position == rhs.position && velocity == rhs.velocity;
     }
@@ -66,7 +75,7 @@ class TrapezoidProfile {
    * @param initial     The initial state (usually the current state).
    */
   TrapezoidProfile(Constraints constraints, State goal,
-                   State initial = State{0_m, 0_mps});
+                   State initial = State{Distance_t(0), Velocity_t(0)});
 
   TrapezoidProfile(const TrapezoidProfile&) = default;
   TrapezoidProfile& operator=(const TrapezoidProfile&) = default;
@@ -86,7 +95,7 @@ class TrapezoidProfile {
    *
    * @param target The target distance.
    */
-  units::second_t TimeLeftUntil(units::meter_t target) const;
+  units::second_t TimeLeftUntil(Distance_t target) const;
 
   /**
    * Returns the total time the profile takes to reach the goal.
@@ -135,5 +144,6 @@ class TrapezoidProfile {
   units::second_t m_endFullSpeed;
   units::second_t m_endDeccel;
 };
-
 }  // namespace frc
+
+#include "TrapezoidProfile.inc"
diff --git a/wpilibc/src/main/native/cpp/trajectory/TrapezoidProfile.cpp b/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.inc
similarity index 76%
rename from wpilibc/src/main/native/cpp/trajectory/TrapezoidProfile.cpp
rename to wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.inc
index 6026aa548d..50f232dca2 100644
--- a/wpilibc/src/main/native/cpp/trajectory/TrapezoidProfile.cpp
+++ b/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.inc
@@ -5,12 +5,14 @@
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
-#include "frc/trajectory/TrapezoidProfile.h"
+#pragma once
 
-using namespace frc;
+#include <algorithm>
 
-TrapezoidProfile::TrapezoidProfile(Constraints constraints, State goal,
-                                   State initial)
+namespace frc {
+template <class Distance>
+TrapezoidProfile<Distance>::TrapezoidProfile(Constraints constraints,
+                                             State goal, State initial)
     : m_direction{ShouldFlipAcceleration(initial, goal) ? -1 : 1},
       m_constraints(constraints),
       m_initial(Direct(initial)),
@@ -24,30 +26,30 @@ TrapezoidProfile::TrapezoidProfile(Constraints constraints, State goal,
   // ended at zero velocity
   units::second_t cutoffBegin =
       m_initial.velocity / m_constraints.maxAcceleration;
-  units::meter_t cutoffDistBegin =
+  Distance_t cutoffDistBegin =
       cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0;
 
   units::second_t cutoffEnd = m_goal.velocity / m_constraints.maxAcceleration;
-  units::meter_t cutoffDistEnd =
+  Distance_t cutoffDistEnd =
       cutoffEnd * cutoffEnd * m_constraints.maxAcceleration / 2.0;
 
   // Now we can calculate the parameters as if it was a full trapezoid instead
   // of a truncated one
 
-  units::meter_t fullTrapezoidDist =
+  Distance_t fullTrapezoidDist =
       cutoffDistBegin + (m_goal.position - m_initial.position) + cutoffDistEnd;
   units::second_t accelerationTime =
       m_constraints.maxVelocity / m_constraints.maxAcceleration;
 
-  units::meter_t fullSpeedDist =
+  Distance_t fullSpeedDist =
       fullTrapezoidDist -
       accelerationTime * accelerationTime * m_constraints.maxAcceleration;
 
   // Handle the case where the profile never reaches full speed
-  if (fullSpeedDist < 0_m) {
+  if (fullSpeedDist < Distance_t(0)) {
     accelerationTime =
         units::math::sqrt(fullTrapezoidDist / m_constraints.maxAcceleration);
-    fullSpeedDist = 0_m;
+    fullSpeedDist = Distance_t(0);
   }
 
   m_endAccel = accelerationTime - cutoffBegin;
@@ -55,7 +57,9 @@ TrapezoidProfile::TrapezoidProfile(Constraints constraints, State goal,
   m_endDeccel = m_endFullSpeed + accelerationTime - cutoffEnd;
 }
 
-TrapezoidProfile::State TrapezoidProfile::Calculate(units::second_t t) const {
+template <class Distance>
+typename TrapezoidProfile<Distance>::State
+TrapezoidProfile<Distance>::Calculate(units::second_t t) const {
   State result = m_initial;
 
   if (t < m_endAccel) {
@@ -83,9 +87,11 @@ TrapezoidProfile::State TrapezoidProfile::Calculate(units::second_t t) const {
   return Direct(result);
 }
 
-units::second_t TrapezoidProfile::TimeLeftUntil(units::meter_t target) const {
-  units::meter_t position = m_initial.position * m_direction;
-  units::meters_per_second_t velocity = m_initial.velocity * m_direction;
+template <class Distance>
+units::second_t TrapezoidProfile<Distance>::TimeLeftUntil(
+    Distance_t target) const {
+  Distance_t position = m_initial.position * m_direction;
+  Velocity_t velocity = m_initial.velocity * m_direction;
 
   units::second_t endAccel = m_endAccel * m_direction;
   units::second_t endFullSpeed = m_endFullSpeed * m_direction - endAccel;
@@ -101,21 +107,19 @@ units::second_t TrapezoidProfile::TimeLeftUntil(units::meter_t target) const {
   units::second_t endDeccel = m_endDeccel - endAccel - endFullSpeed;
   endDeccel = units::math::max(endDeccel, 0_s);
 
-  const units::meters_per_second_squared_t acceleration =
-      m_constraints.maxAcceleration;
-  const units::meters_per_second_squared_t decceleration =
-      -m_constraints.maxAcceleration;
+  const Acceleration_t acceleration = m_constraints.maxAcceleration;
+  const Acceleration_t decceleration = -m_constraints.maxAcceleration;
 
-  units::meter_t distToTarget = units::math::abs(target - position);
+  Distance_t distToTarget = units::math::abs(target - position);
 
-  if (distToTarget < 1e-6_m) {
+  if (distToTarget < Distance_t(1e-6)) {
     return 0_s;
   }
 
-  units::meter_t accelDist =
+  Distance_t accelDist =
       velocity * endAccel + 0.5 * acceleration * endAccel * endAccel;
 
-  units::meters_per_second_t deccelVelocity;
+  Velocity_t deccelVelocity;
   if (endAccel > 0_s) {
     deccelVelocity = units::math::sqrt(
         units::math::abs(velocity * velocity + 2 * acceleration * accelDist));
@@ -123,20 +127,20 @@ units::second_t TrapezoidProfile::TimeLeftUntil(units::meter_t target) const {
     deccelVelocity = velocity;
   }
 
-  units::meter_t deccelDist =
+  Distance_t deccelDist =
       deccelVelocity * endDeccel + 0.5 * decceleration * endDeccel * endDeccel;
 
-  deccelDist = units::math::max(deccelDist, 0_m);
+  deccelDist = units::math::max(deccelDist, Distance_t(0));
 
-  units::meter_t fullSpeedDist = m_constraints.maxVelocity * endFullSpeed;
+  Distance_t fullSpeedDist = m_constraints.maxVelocity * endFullSpeed;
 
   if (accelDist > distToTarget) {
     accelDist = distToTarget;
-    fullSpeedDist = 0_m;
-    deccelDist = 0_m;
+    fullSpeedDist = Distance_t(0);
+    deccelDist = Distance_t(0);
   } else if (accelDist + fullSpeedDist > distToTarget) {
     fullSpeedDist = distToTarget - accelDist;
-    deccelDist = 0_m;
+    deccelDist = Distance_t(0);
   } else {
     deccelDist = distToTarget - fullSpeedDist - accelDist;
   }
@@ -156,3 +160,4 @@ units::second_t TrapezoidProfile::TimeLeftUntil(units::meter_t target) const {
 
   return accelTime + fullSpeedTime + deccelTime;
 }
+}  // namespace frc
diff --git a/wpilibc/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp b/wpilibc/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp
index ddd6a705ea..bd9ffef9ac 100644
--- a/wpilibc/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp
+++ b/wpilibc/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp
@@ -25,12 +25,13 @@ static constexpr auto kDt = 10_ms;
   }
 
 TEST(TrapezoidProfileTest, ReachesGoal) {
-  frc::TrapezoidProfile::Constraints constraints{1.75_mps, 0.75_mps_sq};
-  frc::TrapezoidProfile::State goal{3_m, 0_mps};
-  frc::TrapezoidProfile::State state;
+  frc::TrapezoidProfile<units::meter>::Constraints constraints{1.75_mps,
+                                                               0.75_mps_sq};
+  frc::TrapezoidProfile<units::meter>::State goal{3_m, 0_mps};
+  frc::TrapezoidProfile<units::meter>::State state;
 
   for (int i = 0; i < 450; ++i) {
-    frc::TrapezoidProfile profile{constraints, goal, state};
+    frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
     state = profile.Calculate(kDt);
   }
   EXPECT_EQ(state, goal);
@@ -39,10 +40,11 @@ TEST(TrapezoidProfileTest, ReachesGoal) {
 // Tests that decreasing the maximum velocity in the middle when it is already
 // moving faster than the new max is handled correctly
 TEST(TrapezoidProfileTest, PosContinousUnderVelChange) {
-  frc::TrapezoidProfile::Constraints constraints{1.75_mps, 0.75_mps_sq};
-  frc::TrapezoidProfile::State goal{12_m, 0_mps};
+  frc::TrapezoidProfile<units::meter>::Constraints constraints{1.75_mps,
+                                                               0.75_mps_sq};
+  frc::TrapezoidProfile<units::meter>::State goal{12_m, 0_mps};
 
-  frc::TrapezoidProfile profile{constraints, goal};
+  frc::TrapezoidProfile<units::meter> profile{constraints, goal};
   auto state = profile.Calculate(kDt);
 
   auto lastPos = state.position;
@@ -51,7 +53,7 @@ TEST(TrapezoidProfileTest, PosContinousUnderVelChange) {
       constraints.maxVelocity = 0.75_mps;
     }
 
-    profile = frc::TrapezoidProfile{constraints, goal, state};
+    profile = frc::TrapezoidProfile<units::meter>{constraints, goal, state};
     state = profile.Calculate(kDt);
     auto estimatedVel = (state.position - lastPos) / kDt;
 
@@ -71,31 +73,33 @@ TEST(TrapezoidProfileTest, PosContinousUnderVelChange) {
 
 // There is some somewhat tricky code for dealing with going backwards
 TEST(TrapezoidProfileTest, Backwards) {
-  frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
-  frc::TrapezoidProfile::State goal{-2_m, 0_mps};
-  frc::TrapezoidProfile::State state;
+  frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
+                                                               0.75_mps_sq};
+  frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
+  frc::TrapezoidProfile<units::meter>::State state;
 
   for (int i = 0; i < 400; ++i) {
-    frc::TrapezoidProfile profile{constraints, goal, state};
+    frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
     state = profile.Calculate(kDt);
   }
   EXPECT_EQ(state, goal);
 }
 
 TEST(TrapezoidProfileTest, SwitchGoalInMiddle) {
-  frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
-  frc::TrapezoidProfile::State goal{-2_m, 0_mps};
-  frc::TrapezoidProfile::State state;
+  frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
+                                                               0.75_mps_sq};
+  frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
+  frc::TrapezoidProfile<units::meter>::State state;
 
   for (int i = 0; i < 200; ++i) {
-    frc::TrapezoidProfile profile{constraints, goal, state};
+    frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
     state = profile.Calculate(kDt);
   }
   EXPECT_NE(state, goal);
 
   goal = {0.0_m, 0.0_mps};
   for (int i = 0; i < 550; ++i) {
-    frc::TrapezoidProfile profile{constraints, goal, state};
+    frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
     state = profile.Calculate(kDt);
   }
   EXPECT_EQ(state, goal);
@@ -103,30 +107,32 @@ TEST(TrapezoidProfileTest, SwitchGoalInMiddle) {
 
 // Checks to make sure that it hits top speed
 TEST(TrapezoidProfileTest, TopSpeed) {
-  frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
-  frc::TrapezoidProfile::State goal{4_m, 0_mps};
-  frc::TrapezoidProfile::State state;
+  frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
+                                                               0.75_mps_sq};
+  frc::TrapezoidProfile<units::meter>::State goal{4_m, 0_mps};
+  frc::TrapezoidProfile<units::meter>::State state;
 
   for (int i = 0; i < 200; ++i) {
-    frc::TrapezoidProfile profile{constraints, goal, state};
+    frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
     state = profile.Calculate(kDt);
   }
   EXPECT_NEAR_UNITS(constraints.maxVelocity, state.velocity, 10e-5_mps);
 
   for (int i = 0; i < 2000; ++i) {
-    frc::TrapezoidProfile profile{constraints, goal, state};
+    frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
     state = profile.Calculate(kDt);
   }
   EXPECT_EQ(state, goal);
 }
 
 TEST(TrapezoidProfileTest, TimingToCurrent) {
-  frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
-  frc::TrapezoidProfile::State goal{2_m, 0_mps};
-  frc::TrapezoidProfile::State state;
+  frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
+                                                               0.75_mps_sq};
+  frc::TrapezoidProfile<units::meter>::State goal{2_m, 0_mps};
+  frc::TrapezoidProfile<units::meter>::State state;
 
   for (int i = 0; i < 400; i++) {
-    frc::TrapezoidProfile profile{constraints, goal, state};
+    frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
     state = profile.Calculate(kDt);
     EXPECT_NEAR_UNITS(profile.TimeLeftUntil(state.position), 0_s, 2e-2_s);
   }
@@ -135,16 +141,17 @@ TEST(TrapezoidProfileTest, TimingToCurrent) {
 TEST(TrapezoidProfileTest, TimingToGoal) {
   using units::unit_cast;
 
-  frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
-  frc::TrapezoidProfile::State goal{2_m, 0_mps};
+  frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
+                                                               0.75_mps_sq};
+  frc::TrapezoidProfile<units::meter>::State goal{2_m, 0_mps};
 
-  frc::TrapezoidProfile profile{constraints, goal};
+  frc::TrapezoidProfile<units::meter> profile{constraints, goal};
   auto state = profile.Calculate(kDt);
 
   auto predictedTimeLeft = profile.TimeLeftUntil(goal.position);
   bool reachedGoal = false;
   for (int i = 0; i < 400; i++) {
-    profile = frc::TrapezoidProfile(constraints, goal, state);
+    profile = frc::TrapezoidProfile<units::meter>(constraints, goal, state);
     state = profile.Calculate(kDt);
     if (!reachedGoal && state == goal) {
       // Expected value using for loop index is just an approximation since the
@@ -158,16 +165,17 @@ TEST(TrapezoidProfileTest, TimingToGoal) {
 TEST(TrapezoidProfileTest, TimingBeforeGoal) {
   using units::unit_cast;
 
-  frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
-  frc::TrapezoidProfile::State goal{2_m, 0_mps};
+  frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
+                                                               0.75_mps_sq};
+  frc::TrapezoidProfile<units::meter>::State goal{2_m, 0_mps};
 
-  frc::TrapezoidProfile profile{constraints, goal};
+  frc::TrapezoidProfile<units::meter> profile{constraints, goal};
   auto state = profile.Calculate(kDt);
 
   auto predictedTimeLeft = profile.TimeLeftUntil(1_m);
   bool reachedGoal = false;
   for (int i = 0; i < 400; i++) {
-    profile = frc::TrapezoidProfile(constraints, goal, state);
+    profile = frc::TrapezoidProfile<units::meter>(constraints, goal, state);
     state = profile.Calculate(kDt);
     if (!reachedGoal &&
         (units::math::abs(state.velocity - 1_mps) < 10e-5_mps)) {
@@ -180,16 +188,17 @@ TEST(TrapezoidProfileTest, TimingBeforeGoal) {
 TEST(TrapezoidProfileTest, TimingToNegativeGoal) {
   using units::unit_cast;
 
-  frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
-  frc::TrapezoidProfile::State goal{-2_m, 0_mps};
+  frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
+                                                               0.75_mps_sq};
+  frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
 
-  frc::TrapezoidProfile profile{constraints, goal};
+  frc::TrapezoidProfile<units::meter> profile{constraints, goal};
   auto state = profile.Calculate(kDt);
 
   auto predictedTimeLeft = profile.TimeLeftUntil(goal.position);
   bool reachedGoal = false;
   for (int i = 0; i < 400; i++) {
-    profile = frc::TrapezoidProfile(constraints, goal, state);
+    profile = frc::TrapezoidProfile<units::meter>(constraints, goal, state);
     state = profile.Calculate(kDt);
     if (!reachedGoal && state == goal) {
       // Expected value using for loop index is just an approximation since the
@@ -203,16 +212,17 @@ TEST(TrapezoidProfileTest, TimingToNegativeGoal) {
 TEST(TrapezoidProfileTest, TimingBeforeNegativeGoal) {
   using units::unit_cast;
 
-  frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
-  frc::TrapezoidProfile::State goal{-2_m, 0_mps};
+  frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
+                                                               0.75_mps_sq};
+  frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
 
-  frc::TrapezoidProfile profile{constraints, goal};
+  frc::TrapezoidProfile<units::meter> profile{constraints, goal};
   auto state = profile.Calculate(kDt);
 
   auto predictedTimeLeft = profile.TimeLeftUntil(-1_m);
   bool reachedGoal = false;
   for (int i = 0; i < 400; i++) {
-    profile = frc::TrapezoidProfile(constraints, goal, state);
+    profile = frc::TrapezoidProfile<units::meter>(constraints, goal, state);
     state = profile.Calculate(kDt);
     if (!reachedGoal &&
         (units::math::abs(state.velocity + 1_mps) < 10e-5_mps)) {
diff --git a/wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp/Robot.cpp
index 8479867381..d423dd80a7 100644
--- a/wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp/Robot.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp/Robot.cpp
@@ -41,8 +41,10 @@ class Robot : public frc::TimedRobot {
 
   // Create a PID controller whose setpoint's change is subject to maximum
   // velocity and acceleration constraints.
-  frc::TrapezoidProfile::Constraints m_constraints{1.75_mps, 0.75_mps_sq};
-  frc::ProfiledPIDController m_controller{1.3, 0.0, 0.7, m_constraints, kDt};
+  frc::TrapezoidProfile<units::meters>::Constraints m_constraints{1.75_mps,
+                                                                  0.75_mps_sq};
+  frc::ProfiledPIDController<units::meters> m_controller{1.3, 0.0, 0.7,
+                                                         m_constraints, kDt};
 };
 
 #ifndef RUNNING_FRC_TESTS
diff --git a/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp
index d88331ab04..5a6dcab400 100644
--- a/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp
@@ -31,7 +31,8 @@ class Robot : public frc::TimedRobot {
     // Create a motion profile with the given maximum velocity and maximum
     // acceleration constraints for the next setpoint, the desired goal, and the
     // current setpoint.
-    frc::TrapezoidProfile profile{m_constraints, m_goal, m_setpoint};
+    frc::TrapezoidProfile<units::meter> profile{m_constraints, m_goal,
+                                                m_setpoint};
 
     // Retrieve the profiled setpoint for the next timestep. This setpoint moves
     // toward the goal while obeying the constraints.
@@ -49,9 +50,10 @@ class Robot : public frc::TimedRobot {
   frc::PWMVictorSPX m_motor{1};
   frc2::PIDController m_controller{1.3, 0.0, 0.7, kDt};
 
-  frc::TrapezoidProfile::Constraints m_constraints{1.75_mps, 0.75_mps_sq};
-  frc::TrapezoidProfile::State m_goal;
-  frc::TrapezoidProfile::State m_setpoint;
+  frc::TrapezoidProfile<units::meters>::Constraints m_constraints{1.75_mps,
+                                                                  0.75_mps_sq};
+  frc::TrapezoidProfile<units::meters>::State m_goal;
+  frc::TrapezoidProfile<units::meters>::State m_setpoint;
 };
 
 #ifndef RUNNING_FRC_TESTS
diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp
index 697984ca6f..f7a11b806c 100644
--- a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp
@@ -26,7 +26,8 @@ SwerveModule::SwerveModule(const int driveMotorChannel,
 
   // Limit the PID Controller's input range between -pi and pi and set the input
   // to be continuous.
-  m_turningPIDController.EnableContinuousInput(-wpi::math::pi, wpi::math::pi);
+  m_turningPIDController.EnableContinuousInput(-units::radian_t(wpi::math::pi),
+                                               units::radian_t(wpi::math::pi));
 }
 
 frc::SwerveModuleState SwerveModule::GetState() const {
@@ -41,10 +42,7 @@ void SwerveModule::SetDesiredState(const frc::SwerveModuleState& state) {
 
   // Calculate the turning motor output from the turning PID controller.
   const auto turnOutput = m_turningPIDController.Calculate(
-      units::meter_t(m_turningEncoder.Get()),
-      // We have to convert to the meters type here because that's what
-      // ProfiledPIDController wants.
-      units::meter_t(state.angle.Radians().to<double>()));
+      units::radian_t(m_turningEncoder.Get()), state.angle.Radians());
 
   // Set the motor outputs.
   m_driveMotor.Set(driveOutput);
diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/SwerveModule.h b/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/SwerveModule.h
index eb93d1b69b..0dafa24929 100644
--- a/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/SwerveModule.h
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/SwerveModule.h
@@ -24,15 +24,10 @@ class SwerveModule {
   static constexpr double kWheelRadius = 0.0508;
   static constexpr int kEncoderResolution = 4096;
 
-  // We have to use meters here instead of radians because of the fact that
-  // ProfiledPIDController's constraints only take in meters per second and
-  // meters per second squared.
-
-  static constexpr units::meters_per_second_t kModuleMaxAngularVelocity =
-      units::meters_per_second_t(wpi::math::pi);  // radians per second
-  static constexpr units::meters_per_second_squared_t
-      kModuleMaxAngularAcceleration = units::meters_per_second_squared_t(
-          wpi::math::pi * 2.0);  // radians per second squared
+  static constexpr auto kModuleMaxAngularVelocity =
+      wpi::math::pi * 1_rad_per_s;  // radians per second
+  static constexpr auto kModuleMaxAngularAcceleration =
+      wpi::math::pi * 2_rad_per_s / 1_s;  // radians per second^2
 
   frc::PWMVictorSPX m_driveMotor;
   frc::PWMVictorSPX m_turningMotor;
@@ -41,7 +36,7 @@ class SwerveModule {
   frc::Encoder m_turningEncoder{2, 3};
 
   frc2::PIDController m_drivePIDController{1.0, 0, 0};
-  frc::ProfiledPIDController m_turningPIDController{
+  frc::ProfiledPIDController<units::radians> m_turningPIDController{
       1.0,
       0.0,
       0.0,