From 3207795d0dc1e78a80e16e07852c6bbe8faef416 Mon Sep 17 00:00:00 2001
From: N0tACyb0rg <47341194+N0tACyb0rg@users.noreply.github.com>
Date: Sat, 10 Feb 2024 10:43:23 -0800
Subject: [PATCH] [wpimath] Add lastValue() method to filters (#6351)

---
 .../java/edu/wpi/first/math/filter/LinearFilter.java     | 9 +++++++++
 .../java/edu/wpi/first/math/filter/MedianFilter.java     | 9 +++++++++
 .../java/edu/wpi/first/math/filter/SlewRateLimiter.java  | 9 +++++++++
 .../src/main/native/include/frc/filter/LinearFilter.h    | 7 +++++++
 .../src/main/native/include/frc/filter/MedianFilter.h    | 7 +++++++
 .../src/main/native/include/frc/filter/SlewRateLimiter.h | 7 +++++++
 6 files changed, 48 insertions(+)

diff --git a/wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java b/wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java
index c26270fa49..850b7f2f75 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java
@@ -313,6 +313,15 @@ public class LinearFilter {
     return retVal;
   }
 
+  /**
+   * Returns the last value calculated by the LinearFilter.
+   *
+   * @return The last value.
+   */
+  public double lastValue() {
+    return m_outputs.getFirst();
+  }
+
   /**
    * Factorial of n.
    *
diff --git a/wpimath/src/main/java/edu/wpi/first/math/filter/MedianFilter.java b/wpimath/src/main/java/edu/wpi/first/math/filter/MedianFilter.java
index 13c4e10437..d1d67b5b52 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/filter/MedianFilter.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/filter/MedianFilter.java
@@ -72,6 +72,15 @@ public class MedianFilter {
     }
   }
 
+  /**
+   * Returns the last value calculated by the MedianFilter.
+   *
+   * @return The last value.
+   */
+  public double lastValue() {
+    return m_valueBuffer.getFirst();
+  }
+
   /** Resets the filter, clearing the window of all elements. */
   public void reset() {
     m_orderedValues.clear();
diff --git a/wpimath/src/main/java/edu/wpi/first/math/filter/SlewRateLimiter.java b/wpimath/src/main/java/edu/wpi/first/math/filter/SlewRateLimiter.java
index 6c34f35bc0..d9eca58419 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/filter/SlewRateLimiter.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/filter/SlewRateLimiter.java
@@ -64,6 +64,15 @@ public class SlewRateLimiter {
     return m_prevVal;
   }
 
+  /**
+   * Returns the value last calculated by the SlewRateLimiter.
+   *
+   * @return The last value.
+   */
+  public double lastValue() {
+    return m_prevVal;
+  }
+
   /**
    * Resets the slew rate limiter to the specified value; ignores the rate limit when doing so.
    *
diff --git a/wpimath/src/main/native/include/frc/filter/LinearFilter.h b/wpimath/src/main/native/include/frc/filter/LinearFilter.h
index 051a6ddd26..3bc5465a3d 100644
--- a/wpimath/src/main/native/include/frc/filter/LinearFilter.h
+++ b/wpimath/src/main/native/include/frc/filter/LinearFilter.h
@@ -371,6 +371,13 @@ class LinearFilter {
     return retVal;
   }
 
+  /**
+   * Returns the last value calculated by the LinearFilter.
+   *
+   * @return The last value.
+   */
+  T LastValue() const { return m_outputs.front(); }
+
  private:
   wpi::circular_buffer<T> m_inputs;
   wpi::circular_buffer<T> m_outputs;
diff --git a/wpimath/src/main/native/include/frc/filter/MedianFilter.h b/wpimath/src/main/native/include/frc/filter/MedianFilter.h
index 40422a6d24..4c714fb547 100644
--- a/wpimath/src/main/native/include/frc/filter/MedianFilter.h
+++ b/wpimath/src/main/native/include/frc/filter/MedianFilter.h
@@ -61,6 +61,13 @@ class MedianFilter {
     }
   }
 
+  /**
+   * Returns the last value calculated by the MedianFilter.
+   *
+   * @return The last value.
+   */
+  T LastValue() const { return m_valueBuffer.front(); }
+
   /**
    * Resets the filter, clearing the window of all elements.
    */
diff --git a/wpimath/src/main/native/include/frc/filter/SlewRateLimiter.h b/wpimath/src/main/native/include/frc/filter/SlewRateLimiter.h
index 9f7c6744b9..ceba4ca2bc 100644
--- a/wpimath/src/main/native/include/frc/filter/SlewRateLimiter.h
+++ b/wpimath/src/main/native/include/frc/filter/SlewRateLimiter.h
@@ -75,6 +75,13 @@ class SlewRateLimiter {
     return m_prevVal;
   }
 
+  /**
+   * Returns the value last calculated by the SlewRateLimiter.
+   *
+   * @return The last value.
+   */
+  Unit_t LastValue() const { return m_prevVal; }
+
   /**
    * Resets the slew rate limiter to the specified value; ignores the rate limit
    * when doing so.