From 21b8554082fe66d4e29f45513d5711c6a7447432 Mon Sep 17 00:00:00 2001
From: suryatho <suryathoppae@gmail.com>
Date: Thu, 15 Feb 2024 20:58:20 -0500
Subject: [PATCH] Check for setpoint and goal equal in arm

---
 .../frc2024/subsystems/superstructure/arm/Arm.java            | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java
index 39006bcb..cbcc1a7e 100644
--- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java
+++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java
@@ -21,6 +21,7 @@
 import lombok.RequiredArgsConstructor;
 import lombok.Setter;
 import org.littletonrobotics.frc2024.RobotState;
+import org.littletonrobotics.frc2024.util.EqualsUtil;
 import org.littletonrobotics.frc2024.util.LoggedTunableNumber;
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
@@ -158,8 +159,7 @@ public Rotation2d getSetpoint() {
 
   @AutoLogOutput(key = "Arm/AtGoal")
   public boolean atGoal() {
-    return Math.abs(setpointState.position - goal.getRads())
-        <= Units.degreesToRadians(toleranceDegrees.get());
+    return EqualsUtil.epsilonEquals(setpointState.position, goal.getRads(), 1e-3);
   }
 
   // public Command getStaticCurrent() {