From a8c465f3fb7d4017c383af506e0a47ae1f268ec5 Mon Sep 17 00:00:00 2001
From: Oliver W <34265013+OliverW10@users.noreply.github.com>
Date: Tue, 17 Jan 2023 03:33:15 +1100
Subject: [PATCH] [wpimath] HolonomicDriveController: Add getters for the
 controllers (#4948)

---
 .../controller/HolonomicDriveController.java  | 27 +++++++++++++++++++
 .../controller/HolonomicDriveController.cpp   | 13 +++++++++
 .../frc/controller/HolonomicDriveController.h | 15 +++++++++++
 3 files changed, 55 insertions(+)

diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java
index e43c6ff3d4..87148a45db 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java
@@ -139,4 +139,31 @@ public class HolonomicDriveController {
   public void setEnabled(boolean enabled) {
     m_enabled = enabled;
   }
+
+  /**
+   * Returns the heading controller.
+   *
+   * @return heading ProfiledPIDController
+   */
+  public ProfiledPIDController getThetaController() {
+    return m_thetaController;
+  }
+
+  /**
+   * Returns the x controller.
+   *
+   * @return X PIDController
+   */
+  public PIDController getXController() {
+    return m_xController;
+  }
+
+  /**
+   * Returns the y controller.
+   *
+   * @return Y PIDController
+   */
+  public PIDController getYController() {
+    return m_yController;
+  }
 }
diff --git a/wpimath/src/main/native/cpp/controller/HolonomicDriveController.cpp b/wpimath/src/main/native/cpp/controller/HolonomicDriveController.cpp
index def723470f..7a4c27bc77 100644
--- a/wpimath/src/main/native/cpp/controller/HolonomicDriveController.cpp
+++ b/wpimath/src/main/native/cpp/controller/HolonomicDriveController.cpp
@@ -79,3 +79,16 @@ ChassisSpeeds HolonomicDriveController::Calculate(
 void HolonomicDriveController::SetEnabled(bool enabled) {
   m_enabled = enabled;
 }
+
+ProfiledPIDController<units::radian>&
+HolonomicDriveController::getThetaController() {
+  return m_thetaController;
+}
+
+PIDController& HolonomicDriveController::getXController() {
+  return m_xController;
+}
+
+PIDController& HolonomicDriveController::getYController() {
+  return m_yController;
+}
diff --git a/wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h b/wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h
index 9a749c6f0d..4831e9ca69 100644
--- a/wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h
+++ b/wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h
@@ -103,6 +103,21 @@ class WPILIB_DLLEXPORT HolonomicDriveController {
    */
   void SetEnabled(bool enabled);
 
+  /**
+   * Returns the rotation ProfiledPIDController
+   */
+  ProfiledPIDController<units::radian>& getThetaController();
+
+  /**
+   * Returns the X PIDController
+   */
+  PIDController& getXController();
+
+  /**
+   * Returns the Y PIDController
+   */
+  PIDController& getYController();
+
  private:
   Pose2d m_poseError;
   Rotation2d m_rotationError;