diff --git a/testing/src/main/java/com/team2813/lib2813/testing/truth/MeasureSubject.java b/testing/src/main/java/com/team2813/lib2813/testing/truth/MeasureSubject.java
new file mode 100644
index 00000000..9a5d3ea6
--- /dev/null
+++ b/testing/src/main/java/com/team2813/lib2813/testing/truth/MeasureSubject.java
@@ -0,0 +1,122 @@
+/*
+Copyright 2026 Prospect Robotics SWENext Club
+
+Licensed under the Apache License, Version 2.0 (the "License");
+you may not use this file except in compliance with the License.
+You may obtain a copy of the License at
+
+http://www.apache.org/licenses/LICENSE-2.0
+
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.
+*/
+package com.team2813.lib2813.testing.truth;
+
+import static com.google.common.base.Preconditions.checkArgument;
+import static com.google.common.truth.Fact.fact;
+import static com.google.common.truth.Fact.simpleFact;
+import static com.google.common.truth.Truth.assertAbout;
+
+import com.google.common.primitives.Doubles;
+import com.google.common.truth.FailureMetadata;
+import com.google.common.truth.Subject;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.Unit;
+import javax.annotation.Nullable;
+
+/**
+ * Truth subject for making assertions about {@link Measure} values.
+ *
+ *
See Writing your own custom subject to learn about
+ * creating custom Truth subjects.
+ *
+ * @param The WPILib Unit type of the {@link Measure}
+ * @since 2.1.0
+ */
+public class MeasureSubject extends Subject {
+ public static MeasureSubject assertThat(@Nullable Measure measure) {
+ return assertAbout(MeasureSubject.measures()).that(measure);
+ }
+
+ public static Subject.Factory, Measure> measures() {
+ return MeasureSubject::new;
+ }
+
+ private final Measure actual;
+
+ private MeasureSubject(FailureMetadata failureMetadata, @Nullable Measure subject) {
+ super(failureMetadata, subject);
+ this.actual = subject;
+ }
+
+ public TolerantComparison> isWithin(Measure tolerance) {
+ return new TolerantComparison>() {
+ @Override
+ public void of(Measure expected) {
+ Measure actual = nonNullActual();
+ checkTolerance(tolerance);
+ if (!equalWithinTolerance(actual, expected, tolerance)) {
+ failWithoutActual(
+ fact("expected", formatUnit(expected)),
+ fact("but was", formatUnit(actual)),
+ fact("outside tolerance", formatUnit(tolerance)));
+ }
+ }
+ };
+ }
+
+ public TolerantComparison> isNotWithin(Measure tolerance) {
+ return new TolerantComparison>() {
+ @Override
+ public void of(Measure expected) {
+ Measure actual = nonNullActual();
+ checkTolerance(tolerance);
+ if (!notEqualWithinTolerance(actual, expected, tolerance)) {
+ failWithoutActual(
+ fact("expected not to be", formatUnit(expected)),
+ fact("but was", formatUnit(actual)),
+ fact("within tolerance", formatUnit(tolerance)));
+ }
+ }
+ };
+ }
+
+ private static boolean equalWithinTolerance(
+ Measure left, Measure right, Measure tolerance) {
+ return Math.abs(left.baseUnitMagnitude() - right.baseUnitMagnitude())
+ <= Math.abs(tolerance.baseUnitMagnitude());
+ }
+
+ private static boolean notEqualWithinTolerance(
+ Measure left, Measure right, Measure tolerance) {
+ double leftD = left.baseUnitMagnitude();
+ double rightD = right.baseUnitMagnitude();
+ if (Doubles.isFinite(leftD) && Doubles.isFinite(rightD)) {
+ return Math.abs(leftD - rightD) > Math.abs(tolerance.baseUnitMagnitude());
+ } else {
+ return false;
+ }
+ }
+
+ private static String formatUnit(Measure measure) {
+ return String.format("%g %s", measure.magnitude(), measure.unit().name());
+ }
+
+ private void checkTolerance(Measure tolerance) {
+ double mag = tolerance.baseUnitMagnitude();
+ checkArgument(!Double.isNaN(mag), "tolerance cannot be NaN");
+ checkArgument(mag >= 0, "tolerance (%s) cannot be negative", tolerance);
+ checkArgument(
+ mag != Double.POSITIVE_INFINITY, "tolerance cannot be POSITIVE_INFINITY", tolerance);
+ }
+
+ private Measure nonNullActual() {
+ if (actual == null) {
+ failWithActual(simpleFact("expected a non-null Measure"));
+ }
+ return actual;
+ }
+}
diff --git a/testing/src/main/java/com/team2813/lib2813/testing/truth/Rotation2dSubject.java b/testing/src/main/java/com/team2813/lib2813/testing/truth/Rotation2dSubject.java
index 7c11ca3c..ce81bb6c 100644
--- a/testing/src/main/java/com/team2813/lib2813/testing/truth/Rotation2dSubject.java
+++ b/testing/src/main/java/com/team2813/lib2813/testing/truth/Rotation2dSubject.java
@@ -58,6 +58,15 @@ public void of(Rotation2d expected) {
};
}
+ public TolerantComparison isNotWithin(double tolerance) {
+ return new TolerantComparison() {
+ @Override
+ public void of(Rotation2d expected) {
+ getRadians().isNotWithin(tolerance).of(expected.getRadians());
+ }
+ };
+ }
+
public void isZero() {
if (!Rotation2d.kZero.equals(actual)) {
failWithActual(simpleFact("expected to be zero"));
diff --git a/testing/src/test/java/com/team2813/lib2813/testing/truth/MeasureSubjectTest.java b/testing/src/test/java/com/team2813/lib2813/testing/truth/MeasureSubjectTest.java
new file mode 100644
index 00000000..d4a38755
--- /dev/null
+++ b/testing/src/test/java/com/team2813/lib2813/testing/truth/MeasureSubjectTest.java
@@ -0,0 +1,293 @@
+/*
+Copyright 2026 Prospect Robotics SWENext Club
+
+Licensed under the Apache License, Version 2.0 (the "License");
+you may not use this file except in compliance with the License.
+You may obtain a copy of the License at
+
+http://www.apache.org/licenses/LICENSE-2.0
+
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.
+*/
+package com.team2813.lib2813.testing.truth;
+
+import static com.google.common.truth.ExpectFailure.assertThat;
+import static com.google.common.truth.Truth.assertThat;
+import static edu.wpi.first.units.Units.Volts;
+import static org.junit.jupiter.api.Assertions.*;
+
+import com.google.common.truth.ExpectFailure;
+import edu.wpi.first.units.measure.Voltage;
+import org.junit.jupiter.api.Test;
+
+/** Tests for {@link MeasureSubject}. */
+class MeasureSubjectTest {
+
+ @Test
+ public void isWithin_toleranceIsNegative_throwsIllegalArgumentException() {
+ Voltage expected = Volts.of(12);
+ Voltage actual = Volts.of(12.001);
+ Voltage tolerance = Volts.of(-0.01);
+
+ IllegalArgumentException e =
+ assertThrows(
+ IllegalArgumentException.class,
+ () -> MeasureSubject.assertThat(actual).isWithin(tolerance).of(expected));
+ assertThat(e).hasMessageThat().contains("negative");
+ }
+
+ @Test
+ public void isWithin_toleranceIsNan_throwsIllegalArgumentException() {
+ Voltage expected = Volts.of(12);
+ Voltage actual = Volts.of(12.001);
+ Voltage tolerance = Volts.of(Double.NaN);
+
+ IllegalArgumentException e =
+ assertThrows(
+ IllegalArgumentException.class,
+ () -> MeasureSubject.assertThat(actual).isWithin(tolerance).of(expected));
+ assertThat(e).hasMessageThat().contains("NaN");
+ }
+
+ @Test
+ public void isWithin_toleranceIsInfinity_throwsIllegalArgumentException() {
+ Voltage expected = Volts.of(12);
+ Voltage actual = Volts.of(12.001);
+ Voltage tolerance = Volts.of(Double.POSITIVE_INFINITY);
+
+ IllegalArgumentException e =
+ assertThrows(
+ IllegalArgumentException.class,
+ () -> MeasureSubject.assertThat(actual).isWithin(tolerance).of(expected));
+ assertThat(e).hasMessageThat().contains("POSITIVE_INFINITY");
+ }
+
+ @Test
+ public void isWithin_nullActual_throws() {
+ Voltage expected = Volts.of(12);
+ Voltage actual = null;
+ Voltage tolerance = Volts.of(0.01);
+
+ AssertionError e =
+ assertThrows(
+ AssertionError.class,
+ () -> MeasureSubject.assertThat(actual).isWithin(tolerance).of(expected));
+ assertThat(e).hasMessageThat().contains("non-null");
+ }
+
+ @Test
+ public void isWithin_valueWithinTolerance_doesNotThrow() {
+ Voltage expected = Volts.of(12);
+ Voltage actual = Volts.of(12.001);
+ Voltage tolerance = Volts.of(0.01);
+
+ MeasureSubject.assertThat(actual).isWithin(tolerance).of(expected);
+ }
+
+ @Test
+ public void isWithin_valueNotWithinTolerance_throws() {
+ Voltage expected = Volts.of(12.1);
+ Voltage actual = Volts.of(12.2);
+ Voltage tolerance = Volts.of(0.001);
+
+ AssertionError e =
+ assertThrows(
+ AssertionError.class,
+ () -> MeasureSubject.assertThat(actual).isWithin(tolerance).of(expected));
+ assertThat(e).factKeys().containsExactly("expected", "but was", "outside tolerance");
+ assertThat(e).factValue("expected").matches("12\\.1.*Volt");
+ assertThat(e).factValue("but was").matches("12\\.2.*Volt");
+ assertThat(e).factValue("outside tolerance").matches("0\\.001.*Volt");
+ }
+
+ @Test
+ public void isWithin_actualPositiveInfinity_throws() {
+ Voltage expected = Volts.of(12.1);
+ Voltage actual = Volts.of(Double.POSITIVE_INFINITY);
+ Voltage tolerance = Volts.of(0.1);
+
+ AssertionError e =
+ assertThrows(
+ AssertionError.class,
+ () -> MeasureSubject.assertThat(actual).isWithin(tolerance).of(expected));
+ ExpectFailure.assertThat(e)
+ .factKeys()
+ .containsExactly("expected", "but was", "outside tolerance");
+ ExpectFailure.assertThat(e).factValue("expected").matches("12\\.1.*Volt");
+ ExpectFailure.assertThat(e).factValue("but was").matches("Infinity Volt");
+ ExpectFailure.assertThat(e).factValue("outside tolerance").matches("0\\.1.*Volt");
+ }
+
+ @Test
+ public void isWithin_expectedPositiveInfinity_throws() {
+ Voltage expected = Volts.of(Double.POSITIVE_INFINITY);
+ Voltage actual = Volts.of(12.1);
+ Voltage tolerance = Volts.of(0.1);
+
+ AssertionError e =
+ assertThrows(
+ AssertionError.class,
+ () -> MeasureSubject.assertThat(actual).isWithin(tolerance).of(expected));
+ ExpectFailure.assertThat(e)
+ .factKeys()
+ .containsExactly("expected", "but was", "outside tolerance");
+ ExpectFailure.assertThat(e).factValue("expected").matches("Infinity Volt");
+ ExpectFailure.assertThat(e).factValue("but was").matches("12\\.1.*Volt");
+ ExpectFailure.assertThat(e).factValue("outside tolerance").matches("0\\.1.*Volt");
+ }
+
+ @Test
+ public void isWithin_bothPositiveInfinity_throws() {
+ Voltage expected = Volts.of(Double.POSITIVE_INFINITY);
+ Voltage actual = Volts.of(Double.POSITIVE_INFINITY);
+ Voltage tolerance = Volts.of(0.1);
+
+ AssertionError e =
+ assertThrows(
+ AssertionError.class,
+ () -> MeasureSubject.assertThat(actual).isWithin(tolerance).of(expected));
+ ExpectFailure.assertThat(e)
+ .factKeys()
+ .containsExactly("expected", "but was", "outside tolerance");
+ ExpectFailure.assertThat(e).factValue("expected").matches("Infinity Volt");
+ ExpectFailure.assertThat(e).factValue("but was").matches("Infinity Volt");
+ ExpectFailure.assertThat(e).factValue("outside tolerance").matches("0\\.1.*Volt");
+ }
+
+ @Test
+ public void isNotWithin_toleranceIsNegative_throwsIllegalArgumentException() {
+ Voltage expected = Volts.of(12);
+ Voltage actual = Volts.of(12.001);
+ Voltage tolerance = Volts.of(-0.01);
+
+ IllegalArgumentException e =
+ assertThrows(
+ IllegalArgumentException.class,
+ () -> MeasureSubject.assertThat(actual).isNotWithin(tolerance).of(expected));
+ assertThat(e).hasMessageThat().contains("negative");
+ }
+
+ @Test
+ public void isNotWithin_toleranceIsNan_throwsIllegalArgumentException() {
+ Voltage expected = Volts.of(12);
+ Voltage actual = Volts.of(12.001);
+ Voltage tolerance = Volts.of(Double.NaN);
+
+ IllegalArgumentException e =
+ assertThrows(
+ IllegalArgumentException.class,
+ () -> MeasureSubject.assertThat(actual).isNotWithin(tolerance).of(expected));
+ assertThat(e).hasMessageThat().contains("NaN");
+ }
+
+ @Test
+ public void isNotWithin_toleranceIsInfinity_throwsIllegalArgumentException() {
+ Voltage expected = Volts.of(12);
+ Voltage actual = Volts.of(12.001);
+ Voltage tolerance = Volts.of(Double.POSITIVE_INFINITY);
+
+ IllegalArgumentException e =
+ assertThrows(
+ IllegalArgumentException.class,
+ () -> MeasureSubject.assertThat(actual).isNotWithin(tolerance).of(expected));
+ assertThat(e).hasMessageThat().contains("POSITIVE_INFINITY");
+ }
+
+ @Test
+ public void isNotWithin_nullActual_throws() {
+ Voltage expected = Volts.of(12);
+ Voltage actual = null;
+ Voltage tolerance = Volts.of(0.01);
+
+ AssertionError e =
+ assertThrows(
+ AssertionError.class,
+ () -> MeasureSubject.assertThat(actual).isNotWithin(tolerance).of(expected));
+ assertThat(e).hasMessageThat().contains("non-null");
+ }
+
+ @Test
+ public void isNotWithin_valueNotWithinTolerance_doesNotThrow() {
+ Voltage expected = Volts.of(12);
+ Voltage actual = Volts.of(12.1);
+ Voltage tolerance = Volts.of(0.001);
+
+ MeasureSubject.assertThat(actual).isNotWithin(tolerance).of(expected);
+ }
+
+ @Test
+ public void isNotWithin_valueWithinTolerance_throws() {
+ Voltage expected = Volts.of(12.1);
+ Voltage actual = Volts.of(12.01);
+ Voltage tolerance = Volts.of(0.1);
+
+ AssertionError e =
+ assertThrows(
+ AssertionError.class,
+ () -> MeasureSubject.assertThat(actual).isNotWithin(tolerance).of(expected));
+ ExpectFailure.assertThat(e)
+ .factKeys()
+ .containsExactly("expected not to be", "but was", "within tolerance");
+ ExpectFailure.assertThat(e).factValue("expected not to be").matches("12\\.1.*Volt");
+ ExpectFailure.assertThat(e).factValue("but was").matches("12\\.01.*Volt");
+ ExpectFailure.assertThat(e).factValue("within tolerance").matches("0\\.1.*Volt");
+ }
+
+ @Test
+ public void isNotWithin_actualPositiveInfinity_throws() {
+ Voltage expected = Volts.of(12.1);
+ Voltage actual = Volts.of(Double.POSITIVE_INFINITY);
+ Voltage tolerance = Volts.of(0.1);
+
+ AssertionError e =
+ assertThrows(
+ AssertionError.class,
+ () -> MeasureSubject.assertThat(actual).isNotWithin(tolerance).of(expected));
+ ExpectFailure.assertThat(e)
+ .factKeys()
+ .containsExactly("expected not to be", "but was", "within tolerance");
+ ExpectFailure.assertThat(e).factValue("expected not to be").matches("12\\.1.*Volt");
+ ExpectFailure.assertThat(e).factValue("but was").matches("Infinity Volt");
+ ExpectFailure.assertThat(e).factValue("within tolerance").matches("0\\.1.*Volt");
+ }
+
+ @Test
+ public void isNotWithin_expectedPositiveInfinity_throws() {
+ Voltage expected = Volts.of(Double.POSITIVE_INFINITY);
+ Voltage actual = Volts.of(12.1);
+ Voltage tolerance = Volts.of(0.1);
+
+ AssertionError e =
+ assertThrows(
+ AssertionError.class,
+ () -> MeasureSubject.assertThat(actual).isNotWithin(tolerance).of(expected));
+ ExpectFailure.assertThat(e)
+ .factKeys()
+ .containsExactly("expected not to be", "but was", "within tolerance");
+ ExpectFailure.assertThat(e).factValue("expected not to be").matches("Infinity Volt");
+ ExpectFailure.assertThat(e).factValue("but was").matches("12\\.1.*Volt");
+ ExpectFailure.assertThat(e).factValue("within tolerance").matches("0\\.1.*Volt");
+ }
+
+ @Test
+ public void isNotWithin_bothPositiveInfinity_throws() {
+ Voltage expected = Volts.of(Double.POSITIVE_INFINITY);
+ Voltage actual = Volts.of(Double.POSITIVE_INFINITY);
+ Voltage tolerance = Volts.of(0.1);
+
+ AssertionError e =
+ assertThrows(
+ AssertionError.class,
+ () -> MeasureSubject.assertThat(actual).isNotWithin(tolerance).of(expected));
+ ExpectFailure.assertThat(e)
+ .factKeys()
+ .containsExactly("expected not to be", "but was", "within tolerance");
+ ExpectFailure.assertThat(e).factValue("expected not to be").matches("Infinity Volt");
+ ExpectFailure.assertThat(e).factValue("but was").matches("Infinity Volt");
+ ExpectFailure.assertThat(e).factValue("within tolerance").matches("0\\.1.*Volt");
+ }
+}
diff --git a/testing/src/test/java/com/team2813/lib2813/testing/truth/Rotation2dSubjectTest.java b/testing/src/test/java/com/team2813/lib2813/testing/truth/Rotation2dSubjectTest.java
index 1c93357c..98c89a44 100644
--- a/testing/src/test/java/com/team2813/lib2813/testing/truth/Rotation2dSubjectTest.java
+++ b/testing/src/test/java/com/team2813/lib2813/testing/truth/Rotation2dSubjectTest.java
@@ -39,4 +39,20 @@ public void isWithin_valueNotWithinTolerance_throws() {
AssertionError.class,
() -> Rotation2dSubject.assertThat(closeRotation).isWithin(0.01).of(ROTATION));
}
+
+ @Test
+ public void isNotWithin_valueWithinTolerance_throws() {
+ Rotation2d closeRotation = Pose2dComponent.R.add(ROTATION, 0.009);
+
+ assertThrows(
+ AssertionError.class,
+ () -> Rotation2dSubject.assertThat(closeRotation).isNotWithin(0.01).of(ROTATION));
+ }
+
+ @Test
+ public void isNotWithin_valueNotWithinTolerance_doesNotThrow() {
+ Rotation2d closeRotation = Pose2dComponent.R.add(ROTATION, 0.011);
+
+ Rotation2dSubject.assertThat(closeRotation).isNotWithin(0.01).of(ROTATION);
+ }
}