if speeds is 0 in armTeleop, returns, fix to #24?

DeepBlueRobotics · Mar 6, 2024 · 863565c · 863565c
1 parent d14d5ab
commit 863565c
Showing 1 changed file with 3 additions and 1 deletion.
diff --git a/src/main/java/org/carlmontrobotics/commands/ArmTeleop.java b/src/main/java/org/carlmontrobotics/commands/ArmTeleop.java
@@ -49,7 +49,9 @@ public void execute() {
     double speeds = getRequestedSpeeds();
     double currTime = Timer.getFPGATimestamp();
     double deltaT = currTime - lastTime;
-
+    if (speeds == 0){
+      return;
+    }
     double goalArmRad = goalState.position + speeds * deltaT;
     goalArmRad = MathUtil.clamp(goalArmRad, UPPER_ANGLE_LIMIT_RAD, LOWER_ANGLE_LIMIT_RAD);
     goalArmRad = MathUtil.clamp(goalArmRad, armSubsystem.getArmPos() + ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD,