From 1ad04a3ccd47e16744e6b5495f2669ebab094ff5 Mon Sep 17 00:00:00 2001
From: aidnem <99768676+aidnem@users.noreply.github.com>
Date: Sun, 8 Dec 2024 10:53:13 -0500
Subject: [PATCH] Add UnitUtils class with clampMeasurement method (#67)

---
 .../wpilib_interface/UnitUtils.java           | 37 +++++++++++++++++++
 .../main/java/frc/robot/BuildConstants.java   | 12 +++---
 .../src/test/java/UnitUtilsTests.java         | 32 ++++++++++++++++
 3 files changed, 75 insertions(+), 6 deletions(-)
 create mode 100644 wpilib_interface/src/main/java/coppercore/wpilib_interface/UnitUtils.java
 create mode 100644 wpilib_interface/src/test/java/UnitUtilsTests.java

diff --git a/wpilib_interface/src/main/java/coppercore/wpilib_interface/UnitUtils.java b/wpilib_interface/src/main/java/coppercore/wpilib_interface/UnitUtils.java
new file mode 100644
index 0000000..b927480
--- /dev/null
+++ b/wpilib_interface/src/main/java/coppercore/wpilib_interface/UnitUtils.java
@@ -0,0 +1,37 @@
+package coppercore.wpilib_interface;
+
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.Unit;
+
+/** Utilities for dealing with the WPILib units library. */
+public final class UnitUtils {
+    /**
+     * Given a measurement, a minimum value, and a maximum value, return a clamped measurement
+     * within those bounds.
+     *
+     * <p>Note: If for some reason min is greater than max, measure &lt; min takes precedence over
+     * measure &lt; max, which takes precedence over min &lt; measure &lt; max. Therefore, when
+     * debugging, if this function is strangely always returning min it is highly likely there is an
+     * error in the generation of bounds.
+     *
+     * @param <M> A measure, the type of the measures being compoared. This should be able to be
+     *     inferred automatically.
+     * @param <U> The Unit measured by M. This should be able to be inferred automatically.
+     * @param measure The measure being clamped. If it is greater than min and less than max, it
+     *     will be the result.
+     * @param min The minimum value. If measure &lt; min, min will be returned
+     * @param max The maximum value. If measure &gt; max, max will be returned.
+     * @return If measure is within the bounds, measure. If measure is less than min, min. If
+     *     measure is greater than max, max.
+     */
+    public static final <M extends Measure<U>, U extends Unit> M clampMeasure(
+            M measure, M min, M max) {
+        if (measure.lt(min)) {
+            return min;
+        } else if (measure.gt(max)) {
+            return max;
+        }
+
+        return measure;
+    }
+}
diff --git a/wpilib_interface/src/main/java/frc/robot/BuildConstants.java b/wpilib_interface/src/main/java/frc/robot/BuildConstants.java
index 6b7da8d..09c9ba9 100644
--- a/wpilib_interface/src/main/java/frc/robot/BuildConstants.java
+++ b/wpilib_interface/src/main/java/frc/robot/BuildConstants.java
@@ -5,12 +5,12 @@ public final class BuildConstants {
     public static final String MAVEN_GROUP = "io.github.team401.coppercore";
     public static final String MAVEN_NAME = "wpilib_interface";
     public static final String VERSION = "2025.0.0-beta";
-    public static final int GIT_REVISION = 43;
-    public static final String GIT_SHA = "60c0db4808cb5dcf1d9bb3c9a4ba41c86abf1d15";
-    public static final String GIT_DATE = "2024-12-05 12:25:16 EST";
-    public static final String GIT_BRANCH = "2025core";
-    public static final String BUILD_DATE = "2024-12-06 09:44:25 EST";
-    public static final long BUILD_UNIX_TIME = 1733496265888L;
+    public static final int GIT_REVISION = 42;
+    public static final String GIT_SHA = "ae92bd015cfea62f3e15e05ecaa129c58aa316b1";
+    public static final String GIT_DATE = "2024-12-06 12:09:14 EST";
+    public static final String GIT_BRANCH = "64-clamp-measures";
+    public static final String BUILD_DATE = "2024-12-07 23:05:47 EST";
+    public static final long BUILD_UNIX_TIME = 1733630747090L;
     public static final int DIRTY = 1;
 
     private BuildConstants() {}
diff --git a/wpilib_interface/src/test/java/UnitUtilsTests.java b/wpilib_interface/src/test/java/UnitUtilsTests.java
new file mode 100644
index 0000000..b7deb8f
--- /dev/null
+++ b/wpilib_interface/src/test/java/UnitUtilsTests.java
@@ -0,0 +1,32 @@
+import coppercore.wpilib_interface.UnitUtils;
+import edu.wpi.first.units.Units;
+// import edu.wpi.first.units.Units.Meters;
+import edu.wpi.first.units.Units.*;
+import org.junit.jupiter.api.Assertions;
+import org.junit.jupiter.api.Test;
+
+public class UnitUtilsTests {
+    @Test
+    public void clampWithinBounds() {
+        Assertions.assertTrue(
+                UnitUtils.clampMeasure(
+                                Units.Meters.of(2.0), Units.Meters.of(1.0), Units.Meters.of(3.0))
+                        .equals(Units.Meters.of(2.0)));
+    }
+
+    @Test
+    public void clampBelowBounds() {
+        Assertions.assertTrue(
+                UnitUtils.clampMeasure(
+                                Units.Meters.of(0.0), Units.Meters.of(1.0), Units.Meters.of(3.0))
+                        .equals(Units.Meters.of(1.0)));
+    }
+
+    @Test
+    public void clampAboveBounds() {
+        Assertions.assertTrue(
+                UnitUtils.clampMeasure(
+                                Units.Meters.of(4.0), Units.Meters.of(1.0), Units.Meters.of(3.0))
+                        .equals(Units.Meters.of(3.0)));
+    }
+}