From 002aa8d43620fe22cd24802a53c5970411467912 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Fri, 3 Nov 2023 13:10:46 -0700
Subject: [PATCH] [wpimath] Don't recreate TrapezoidProfile in
 ProfiledPIDController calculate()

---
 .../math/controller/ProfiledPIDController.java   |  9 ++++++---
 .../frc/controller/ProfiledPIDController.h       | 16 +++++++++++-----
 2 files changed, 17 insertions(+), 8 deletions(-)

diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ProfiledPIDController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ProfiledPIDController.java
index 6a3862f4fc3..80311f5bfb3 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/ProfiledPIDController.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ProfiledPIDController.java
@@ -22,9 +22,11 @@ public class ProfiledPIDController implements Sendable {
   private PIDController m_controller;
   private double m_minimumInput;
   private double m_maximumInput;
+
+  private TrapezoidProfile.Constraints m_constraints;
+  private TrapezoidProfile m_profile;
   private TrapezoidProfile.State m_goal = new TrapezoidProfile.State();
   private TrapezoidProfile.State m_setpoint = new TrapezoidProfile.State();
-  private TrapezoidProfile.Constraints m_constraints;
 
   /**
    * Allocates a ProfiledPIDController with the given constants for Kp, Ki, and Kd.
@@ -52,6 +54,7 @@ public ProfiledPIDController(
       double Kp, double Ki, double Kd, TrapezoidProfile.Constraints constraints, double period) {
     m_controller = new PIDController(Kp, Ki, Kd, period);
     m_constraints = constraints;
+    m_profile = new TrapezoidProfile(m_constraints);
     instances++;
 
     SendableRegistry.add(this, "ProfiledPIDController", instances);
@@ -219,6 +222,7 @@ public boolean atGoal() {
    */
   public void setConstraints(TrapezoidProfile.Constraints constraints) {
     m_constraints = constraints;
+    m_profile = new TrapezoidProfile(m_constraints);
   }
 
   /**
@@ -343,8 +347,7 @@ public double calculate(double measurement) {
       m_setpoint.position = setpointMinDistance + measurement;
     }
 
-    var profile = new TrapezoidProfile(m_constraints);
-    m_setpoint = profile.calculate(getPeriod(), m_goal, m_setpoint);
+    m_setpoint = m_profile.calculate(getPeriod(), m_goal, m_setpoint);
     return m_controller.calculate(measurement, m_setpoint.position);
   }
 
diff --git a/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h b/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h
index cba1f740d2b..8f211c62c4c 100644
--- a/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h
+++ b/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h
@@ -58,7 +58,9 @@ class ProfiledPIDController
    */
   ProfiledPIDController(double Kp, double Ki, double Kd,
                         Constraints constraints, units::second_t period = 20_ms)
-      : m_controller(Kp, Ki, Kd, period), m_constraints(constraints) {
+      : m_controller{Kp, Ki, Kd, period},
+        m_constraints{constraints},
+        m_profile{m_constraints} {
     int instances = detail::IncrementAndGetProfiledPIDControllerInstances();
     wpi::math::MathSharedStore::ReportUsage(
         wpi::math::MathUsageId::kController_ProfiledPIDController, instances);
@@ -202,7 +204,10 @@ class ProfiledPIDController
    *
    * @param constraints Velocity and acceleration constraints for goal.
    */
-  void SetConstraints(Constraints constraints) { m_constraints = constraints; }
+  void SetConstraints(Constraints constraints) {
+    m_constraints = constraints;
+    m_profile = TrapezoidProfile<Distance>{m_constraints};
+  }
 
   /**
    * Get the velocity and acceleration constraints for this controller.
@@ -317,8 +322,7 @@ class ProfiledPIDController
       m_setpoint.position = setpointMinDistance + measurement;
     }
 
-    frc::TrapezoidProfile<Distance> profile{m_constraints};
-    m_setpoint = profile.Calculate(GetPeriod(), m_goal, m_setpoint);
+    m_setpoint = m_profile.Calculate(GetPeriod(), m_goal, m_setpoint);
     return m_controller.Calculate(measurement.value(),
                                   m_setpoint.position.value());
   }
@@ -408,9 +412,11 @@ class ProfiledPIDController
   PIDController m_controller;
   Distance_t m_minimumInput{0};
   Distance_t m_maximumInput{0};
+
+  typename frc::TrapezoidProfile<Distance>::Constraints m_constraints;
+  TrapezoidProfile<Distance> m_profile;
   typename frc::TrapezoidProfile<Distance>::State m_goal;
   typename frc::TrapezoidProfile<Distance>::State m_setpoint;
-  typename frc::TrapezoidProfile<Distance>::Constraints m_constraints;
 };
 
 }  // namespace frc