From 8ff445e30ec2ce32955624ddc67c63dfdcda65e0 Mon Sep 17 00:00:00 2001
From: Brandon <brandonshen123@gmail.com>
Date: Sat, 2 Mar 2024 21:52:30 -0800
Subject: [PATCH] armAtSetpoint()

---
 src/main/java/org/carlmontrobotics/subsystems/Arm.java | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java
index 6d3ff393..745b45f6 100644
--- a/src/main/java/org/carlmontrobotics/subsystems/Arm.java
+++ b/src/main/java/org/carlmontrobotics/subsystems/Arm.java
@@ -233,7 +233,8 @@ public TrapezoidProfile.State getCurrentArmGoal() {
 
    
     public boolean armAtSetpoint() {
-        return armProfile.isFinished(armProfileTimer.get());
+        return Math.abs(getArmPos() - goalState.position) < POS_TOLERANCE_RAD &&
+                Math.abs(getArmVel() - goalState.velocity) < VEL_TOLERANCE_RAD_P_SEC;
     }