armAtSetpoint()

DeepBlueRobotics · Mar 3, 2024 · 8ff445e · 8ff445e
1 parent 8fe76e7
commit 8ff445e
Showing 1 changed file with 2 additions and 1 deletion.
diff --git 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;
     }