From 7ba8a9ee1f058468bb3006a4f05e89d319cc96d3 Mon Sep 17 00:00:00 2001
From: shueja-personal <32416547+shueja-personal@users.noreply.github.com>
Date: Tue, 13 Dec 2022 22:31:06 -0800
Subject: [PATCH] [wpimath] ProfiledPIDController: Add to SendableRegistry
 (#4656)

Co-authored-by: Ryan Blue <ryanzblue@gmail.com>
---
 .../wpi/first/math/controller/ProfiledPIDController.java   | 3 +++
 .../main/native/cpp/controller/ProfiledPIDController.cpp   | 6 ++----
 .../native/include/frc/controller/ProfiledPIDController.h  | 7 +++++--
 3 files changed, 10 insertions(+), 6 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 681e3fc54b..74649fdb9b 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
@@ -10,6 +10,7 @@ import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.util.sendable.Sendable;
 import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
 
 /**
  * Implements a PID control loop whose setpoint is constrained by a trapezoid profile. Users should
@@ -52,6 +53,8 @@ public class ProfiledPIDController implements Sendable {
     m_controller = new PIDController(Kp, Ki, Kd, period);
     m_constraints = constraints;
     instances++;
+
+    SendableRegistry.add(this, "ProfiledPIDController", instances);
     MathSharedStore.reportUsage(MathUsageId.kController_ProfiledPIDController, instances);
   }
 
diff --git a/wpimath/src/main/native/cpp/controller/ProfiledPIDController.cpp b/wpimath/src/main/native/cpp/controller/ProfiledPIDController.cpp
index fa5842763c..be678cb118 100644
--- a/wpimath/src/main/native/cpp/controller/ProfiledPIDController.cpp
+++ b/wpimath/src/main/native/cpp/controller/ProfiledPIDController.cpp
@@ -4,9 +4,7 @@
 
 #include "frc/controller/ProfiledPIDController.h"
 
-void frc::detail::ReportProfiledPIDController() {
+int frc::detail::IncrementAndGetProfiledPIDControllerInstances() {
   static int instances = 0;
-  ++instances;
-  wpi::math::MathSharedStore::ReportUsage(
-      wpi::math::MathUsageId::kController_ProfiledPIDController, instances);
+  return ++instances;
 }
diff --git a/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h b/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h
index 086c53ebe0..8491118083 100644
--- a/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h
+++ b/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h
@@ -22,7 +22,7 @@
 namespace frc {
 namespace detail {
 WPILIB_DLLEXPORT
-void ReportProfiledPIDController();
+int IncrementAndGetProfiledPIDControllerInstances();
 }  // namespace detail
 
 /**
@@ -59,7 +59,10 @@ 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) {
-    detail::ReportProfiledPIDController();
+    int instances = detail::IncrementAndGetProfiledPIDControllerInstances();
+    wpi::math::MathSharedStore::ReportUsage(
+        wpi::math::MathUsageId::kController_ProfiledPIDController, instances);
+    wpi::SendableRegistry::Add(this, "ProfiledPIDController", instances);
   }
 
   ~ProfiledPIDController() override = default;