From 45f3e3a069bdc6bb8b1f9a1e32009cef19648267 Mon Sep 17 00:00:00 2001
From: Brendan Raykoff <99614905+Braykoff@users.noreply.github.com>
Date: Sun, 21 Jul 2024 02:17:13 -0400
Subject: [PATCH] [wpimath] Add getAccumulatedError() to PIDController (#6813)

---
 .../edu/wpi/first/math/controller/PIDController.java   |  9 +++++++++
 .../first/math/controller/ProfiledPIDController.java   |  9 +++++++++
 .../src/main/native/cpp/controller/PIDController.cpp   |  4 ++++
 .../main/native/include/frc/controller/PIDController.h |  8 ++++++++
 .../include/frc/controller/ProfiledPIDController.h     | 10 ++++++++++
 5 files changed, 40 insertions(+)

diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java
index ce44152065..b55813bfa1 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java
@@ -241,6 +241,15 @@ public class PIDController implements Sendable, AutoCloseable {
     return m_velocityTolerance;
   }
 
+  /**
+   * Returns the accumulated error used in the integral calculation of this controller.
+   *
+   * @return The accumulated error of this controller.
+   */
+  public double getAccumulatedError() {
+    return m_totalError;
+  }
+
   /**
    * Sets the setpoint for the PIDController.
    *
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 f54849a1e5..4f6c3da1f5 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
@@ -186,6 +186,15 @@ public class ProfiledPIDController implements Sendable {
     return m_controller.getVelocityTolerance();
   }
 
+  /**
+   * Returns the accumulated error used in the integral calculation of this controller.
+   *
+   * @return The accumulated error of this controller.
+   */
+  public double getAccumulatedError() {
+    return m_controller.getAccumulatedError();
+  }
+
   /**
    * Sets the goal for the ProfiledPIDController.
    *
diff --git a/wpimath/src/main/native/cpp/controller/PIDController.cpp b/wpimath/src/main/native/cpp/controller/PIDController.cpp
index a67d56d0ea..1e8bfc2aec 100644
--- a/wpimath/src/main/native/cpp/controller/PIDController.cpp
+++ b/wpimath/src/main/native/cpp/controller/PIDController.cpp
@@ -110,6 +110,10 @@ double PIDController::GetVelocityTolerance() const {
   return m_velocityTolerance;
 }
 
+double PIDController::GetAccumulatedError() const {
+  return m_totalError;
+}
+
 void PIDController::SetSetpoint(double setpoint) {
   m_setpoint = setpoint;
   m_haveSetpoint = true;
diff --git a/wpimath/src/main/native/include/frc/controller/PIDController.h b/wpimath/src/main/native/include/frc/controller/PIDController.h
index e8d923ad70..90687e9344 100644
--- a/wpimath/src/main/native/include/frc/controller/PIDController.h
+++ b/wpimath/src/main/native/include/frc/controller/PIDController.h
@@ -135,6 +135,14 @@ class WPILIB_DLLEXPORT PIDController
    */
   double GetVelocityTolerance() const;
 
+  /**
+   * Gets the accumulated error used in the integral calculation of this
+   * controller.
+   *
+   * @return The accumulated error of this controller.
+   */
+  double GetAccumulatedError() const;
+
   /**
    * Sets the setpoint for the PIDController.
    *
diff --git a/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h b/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h
index 10676622cf..bd5866f133 100644
--- a/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h
+++ b/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h
@@ -174,6 +174,16 @@ class ProfiledPIDController
     return m_controller.GetVelocityTolerance();
   }
 
+  /**
+   * Gets the accumulated error used in the integral calculation of this
+   * controller.
+   *
+   * @return The accumulated error of this controller.
+   */
+  double GetAccumulatedError() const {
+    return m_controller.GetAccumulatedError();
+  }
+
   /**
    * Sets the goal for the ProfiledPIDController.
    *