diff --git a/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java b/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java
index 9e74aba762..3785780a8d 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java
@@ -146,6 +146,26 @@ public final class MathUtil {
     return startValue + (endValue - startValue) * MathUtil.clamp(t, 0, 1);
   }
 
+  /**
+   * Return where within interpolation range [0, 1] q is between startValue and endValue.
+   *
+   * @param startValue Lower part of interpolation range.
+   * @param endValue Upper part of interpolation range.
+   * @param q Query.
+   * @return Interpolant in range [0, 1].
+   */
+  public static double inverseInterpolate(double startValue, double endValue, double q) {
+    double totalRange = endValue - startValue;
+    if (totalRange <= 0) {
+      return 0.0;
+    }
+    double queryToStart = q - startValue;
+    if (queryToStart <= 0) {
+      return 0.0;
+    }
+    return queryToStart / totalRange;
+  }
+
   /**
    * Checks if the given value matches an expected value within a certain tolerance.
    *
diff --git a/wpimath/src/main/java/edu/wpi/first/math/interpolation/InterpolatingDoubleTreeMap.java b/wpimath/src/main/java/edu/wpi/first/math/interpolation/InterpolatingDoubleTreeMap.java
new file mode 100644
index 0000000000..046de473b3
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/interpolation/InterpolatingDoubleTreeMap.java
@@ -0,0 +1,15 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.interpolation;
+
+/**
+ * Interpolating Tree Maps are used to get values at points that are not defined by making a guess
+ * from points that are defined. This uses linear interpolation.
+ */
+public class InterpolatingDoubleTreeMap extends InterpolatingTreeMap<Double, Double> {
+  InterpolatingDoubleTreeMap() {
+    super(InverseInterpolator.forDouble(), Interpolator.forDouble());
+  }
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/interpolation/InterpolatingTreeMap.java b/wpimath/src/main/java/edu/wpi/first/math/interpolation/InterpolatingTreeMap.java
new file mode 100644
index 0000000000..22eb2326d9
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/interpolation/InterpolatingTreeMap.java
@@ -0,0 +1,104 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.interpolation;
+
+import java.util.Comparator;
+import java.util.TreeMap;
+
+/**
+ * Interpolating Tree Maps are used to get values at points that are not defined by making a guess
+ * from points that are defined. This uses linear interpolation.
+ *
+ * <p>{@code K} must implement {@link Comparable}, or a {@link Comparator} on {@code K} can be
+ * provided.
+ *
+ * @param <K> The type of keys held in this map.
+ * @param <V> The type of values held in this map.
+ */
+public class InterpolatingTreeMap<K, V> {
+  private final TreeMap<K, V> m_map;
+
+  private final InverseInterpolator<K> m_inverseInterpolator;
+  private final Interpolator<V> m_interpolator;
+
+  /**
+   * Constructs an InterpolatingTreeMap.
+   *
+   * @param inverseInterpolator Function to use for inverse interpolation of the keys.
+   * @param interpolator Function to use for interpolation of the values.
+   */
+  public InterpolatingTreeMap(
+      InverseInterpolator<K> inverseInterpolator, Interpolator<V> interpolator) {
+    m_map = new TreeMap<>();
+    m_inverseInterpolator = inverseInterpolator;
+    m_interpolator = interpolator;
+  }
+
+  /**
+   * Constructs an InterpolatingTreeMap using {@code comparator}.
+   *
+   * @param inverseInterpolator Function to use for inverse interpolation of the keys.
+   * @param interpolator Function to use for interpolation of the values.
+   * @param comparator Comparator to use on keys.
+   */
+  public InterpolatingTreeMap(
+      InverseInterpolator<K> inverseInterpolator,
+      Interpolator<V> interpolator,
+      Comparator<K> comparator) {
+    m_inverseInterpolator = inverseInterpolator;
+    m_interpolator = interpolator;
+    m_map = new TreeMap<>(comparator);
+  }
+
+  /**
+   * Inserts a key-value pair.
+   *
+   * @param key The key.
+   * @param value The value.
+   */
+  public void put(K key, V value) {
+    m_map.put(key, value);
+  }
+
+  /**
+   * Returns the value associated with a given key.
+   *
+   * <p>If there's no matching key, the value returned will be an interpolation between the keys
+   * before and after the provided one, using the {@link Interpolator} and {@link
+   * InverseInterpolator} provided.
+   *
+   * @param key The key.
+   * @return The value associated with the given key.
+   */
+  public V get(K key) {
+    V val = m_map.get(key);
+    if (val == null) {
+      K ceilingKey = m_map.ceilingKey(key);
+      K floorKey = m_map.floorKey(key);
+
+      if (ceilingKey == null && floorKey == null) {
+        return null;
+      }
+      if (ceilingKey == null) {
+        return m_map.get(floorKey);
+      }
+      if (floorKey == null) {
+        return m_map.get(ceilingKey);
+      }
+      V floor = m_map.get(floorKey);
+      V ceiling = m_map.get(ceilingKey);
+
+      return m_interpolator.interpolate(
+          floor, ceiling, m_inverseInterpolator.inverseInterpolate(floorKey, ceilingKey, key));
+    } else {
+      return val;
+    }
+  }
+
+  /** Clears the contents. */
+  public void clear() {
+    m_map.clear();
+  }
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/interpolation/Interpolator.java b/wpimath/src/main/java/edu/wpi/first/math/interpolation/Interpolator.java
new file mode 100644
index 0000000000..be6d8a2d26
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/interpolation/Interpolator.java
@@ -0,0 +1,30 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.interpolation;
+
+import edu.wpi.first.math.MathUtil;
+
+/**
+ * An interpolation function that returns a value interpolated between an upper and lower bound.
+ * This behavior can be linear or nonlinear.
+ *
+ * @param <T> The type that the {@link Interpolator} will operate on.
+ */
+@FunctionalInterface
+public interface Interpolator<T> {
+  /**
+   * Perform interpolation between two values.
+   *
+   * @param startValue The value to start at.
+   * @param endValue The value to end at.
+   * @param t How far between the two values to interpolate. This should be bounded to [0, 1].
+   * @return The interpolated value.
+   */
+  T interpolate(T startValue, T endValue, double t);
+
+  static Interpolator<Double> forDouble() {
+    return MathUtil::interpolate;
+  }
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/interpolation/InverseInterpolator.java b/wpimath/src/main/java/edu/wpi/first/math/interpolation/InverseInterpolator.java
new file mode 100644
index 0000000000..8278af3f45
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/interpolation/InverseInterpolator.java
@@ -0,0 +1,30 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.interpolation;
+
+import edu.wpi.first.math.MathUtil;
+
+/**
+ * An inverse interpolation function which determines where within an interpolation range an object
+ * lies. This behavior can be linear or nonlinear.
+ *
+ * @param <T> The type that the {@link InverseInterpolator} will operate on.
+ */
+@FunctionalInterface
+public interface InverseInterpolator<T> {
+  /**
+   * Return where within interpolation range [0, 1] q is between startValue and endValue.
+   *
+   * @param startValue Lower part of interpolation range.
+   * @param endValue Upper part of interpolation range.
+   * @param q Query.
+   * @return Interpolant in range [0, 1].
+   */
+  double inverseInterpolate(T startValue, T endValue, T q);
+
+  static InverseInterpolator<Double> forDouble() {
+    return MathUtil::inverseInterpolate;
+  }
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/interpolation/TimeInterpolatableBuffer.java b/wpimath/src/main/java/edu/wpi/first/math/interpolation/TimeInterpolatableBuffer.java
index 7e0712d42a..f9f20c3732 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/interpolation/TimeInterpolatableBuffer.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/interpolation/TimeInterpolatableBuffer.java
@@ -19,11 +19,10 @@ import java.util.TreeMap;
  */
 public final class TimeInterpolatableBuffer<T> {
   private final double m_historySize;
-  private final InterpolateFunction<T> m_interpolatingFunc;
+  private final Interpolator<T> m_interpolatingFunc;
   private final NavigableMap<Double, T> m_pastSnapshots = new TreeMap<>();
 
-  private TimeInterpolatableBuffer(
-      InterpolateFunction<T> interpolateFunction, double historySizeSeconds) {
+  private TimeInterpolatableBuffer(Interpolator<T> interpolateFunction, double historySizeSeconds) {
     this.m_historySize = historySizeSeconds;
     this.m_interpolatingFunc = interpolateFunction;
   }
@@ -37,7 +36,7 @@ public final class TimeInterpolatableBuffer<T> {
    * @return The new TimeInterpolatableBuffer.
    */
   public static <T> TimeInterpolatableBuffer<T> createBuffer(
-      InterpolateFunction<T> interpolateFunction, double historySizeSeconds) {
+      Interpolator<T> interpolateFunction, double historySizeSeconds) {
     return new TimeInterpolatableBuffer<>(interpolateFunction, historySizeSeconds);
   }
 
@@ -143,17 +142,4 @@ public final class TimeInterpolatableBuffer<T> {
   public NavigableMap<Double, T> getInternalBuffer() {
     return m_pastSnapshots;
   }
-
-  public interface InterpolateFunction<T> {
-    /**
-     * Return the interpolated value. This object is assumed to be the starting position, or lower
-     * bound.
-     *
-     * @param start The lower bound, or start.
-     * @param end The upper bound, or end.
-     * @param t How far between the lower and upper bound we are. This should be bounded in [0, 1].
-     * @return The interpolated value.
-     */
-    T interpolate(T start, T end, double t);
-  }
 }
diff --git a/wpimath/src/test/java/edu/wpi/first/math/interpolation/InterpolatingDoubleTreeMapTest.java b/wpimath/src/test/java/edu/wpi/first/math/interpolation/InterpolatingDoubleTreeMapTest.java
new file mode 100644
index 0000000000..fb74b92eb9
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/interpolation/InterpolatingDoubleTreeMapTest.java
@@ -0,0 +1,76 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.interpolation;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import org.junit.jupiter.api.Test;
+
+class InterpolatingDoubleTreeMapTest {
+  @Test
+  void testInterpolationDouble() {
+    InterpolatingDoubleTreeMap table = new InterpolatingDoubleTreeMap();
+
+    table.put(125.0, 450.0);
+    table.put(200.0, 510.0);
+    table.put(268.0, 525.0);
+    table.put(312.0, 550.0);
+    table.put(326.0, 650.0);
+
+    // Key below minimum gives the smallest value
+    assertEquals(450.0, table.get(100.0));
+
+    // Minimum key gives exact value
+    assertEquals(450.0, table.get(125.0));
+
+    // Key gives interpolated value
+    assertEquals(480.0, table.get(162.5));
+
+    // Key at right of interpolation range gives exact value
+    assertEquals(510.0, table.get(200.0));
+
+    // Maximum key gives exact value
+    assertEquals(650.0, table.get(326.0));
+
+    // Key above maximum gives largest value
+    assertEquals(650.0, table.get(400.0));
+  }
+
+  @Test
+  void testInterpolationClear() {
+    InterpolatingDoubleTreeMap table = new InterpolatingDoubleTreeMap();
+
+    table.put(125.0, 450.0);
+    table.put(200.0, 510.0);
+    table.put(268.0, 525.0);
+    table.put(312.0, 550.0);
+    table.put(326.0, 650.0);
+
+    // Key below minimum gives the smallest value
+    assertEquals(450.0, table.get(100.0));
+
+    // Minimum key gives exact value
+    assertEquals(450.0, table.get(125.0));
+
+    // Key gives interpolated value
+    assertEquals(480.0, table.get(162.5));
+
+    // Key at right of interpolation range gives exact value
+    assertEquals(510.0, table.get(200.0));
+
+    // Maximum key gives exact value
+    assertEquals(650.0, table.get(326.0));
+
+    // Key above maximum gives largest value
+    assertEquals(650.0, table.get(400.0));
+
+    table.clear();
+
+    table.put(100.0, 250.0);
+    table.put(200.0, 500.0);
+
+    assertEquals(375.0, table.get(150.0));
+  }
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/interpolation/InterpolatingTreeMapTest.java b/wpimath/src/test/java/edu/wpi/first/math/interpolation/InterpolatingTreeMapTest.java
new file mode 100644
index 0000000000..b04b40ab3c
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/interpolation/InterpolatingTreeMapTest.java
@@ -0,0 +1,48 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.interpolation;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import org.junit.jupiter.api.Test;
+
+class InterpolatingTreeMapTest {
+  @Test
+  void testInterpolation() {
+    InterpolatingTreeMap<Double, Double> table =
+        new InterpolatingTreeMap<>(InverseInterpolator.forDouble(), Interpolator.forDouble());
+
+    table.put(125.0, 450.0);
+    table.put(200.0, 510.0);
+    table.put(268.0, 525.0);
+    table.put(312.0, 550.0);
+    table.put(326.0, 650.0);
+
+    // Key below minimum gives the smallest value
+    assertEquals(450.0, table.get(100.0));
+
+    // Minimum key gives exact value
+    assertEquals(450.0, table.get(125.0));
+
+    // Key gives interpolated value
+    assertEquals(480.0, table.get(162.5));
+
+    // Key at right of interpolation range gives exact value
+    assertEquals(510.0, table.get(200.0));
+
+    // Maximum key gives exact value
+    assertEquals(650.0, table.get(326.0));
+
+    // Key above maximum gives largest value
+    assertEquals(650.0, table.get(400.0));
+
+    table.clear();
+
+    table.put(100.0, 250.0);
+    table.put(200.0, 500.0);
+
+    assertEquals(375.0, table.get(150.0));
+  }
+}
diff --git a/wpiutil/src/main/java/edu/wpi/first/util/InterpolatingTreeMap.java b/wpiutil/src/main/java/edu/wpi/first/util/InterpolatingTreeMap.java
index ade2e5a42d..2c54d00ca4 100644
--- a/wpiutil/src/main/java/edu/wpi/first/util/InterpolatingTreeMap.java
+++ b/wpiutil/src/main/java/edu/wpi/first/util/InterpolatingTreeMap.java
@@ -9,7 +9,10 @@ import java.util.TreeMap;
 /**
  * Interpolating Tree Maps are used to get values at points that are not defined by making a guess
  * from points that are defined. This uses linear interpolation.
+ *
+ * @deprecated Use {@link edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap} instead
  */
+@Deprecated(forRemoval = true, since = "2024")
 public class InterpolatingTreeMap<K extends Number, V extends Number> {
   private final TreeMap<K, V> m_map = new TreeMap<>();
 
diff --git a/wpiutil/src/test/java/edu/wpi/first/util/InterpolatingTreeMapTest.java b/wpiutil/src/test/java/edu/wpi/first/util/InterpolatingTreeMapTest.java
index ea23dc5ea1..2203ca81ce 100644
--- a/wpiutil/src/test/java/edu/wpi/first/util/InterpolatingTreeMapTest.java
+++ b/wpiutil/src/test/java/edu/wpi/first/util/InterpolatingTreeMapTest.java
@@ -8,6 +8,7 @@ import static org.junit.jupiter.api.Assertions.assertEquals;
 
 import org.junit.jupiter.api.Test;
 
+@SuppressWarnings("removal")
 class InterpolatingTreeMapTest {
   @Test
   void testInterpolationDouble() {