Check for setpoint and goal equal in arm

Mechanical-Advantage · Feb 16, 2024 · 21b8554 · 21b8554
1 parent 4bca0b1
commit 21b8554
Showing 1 changed file with 2 additions and 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
@@ -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() {