armteleop clamping

DeepBlueRobotics · Mar 3, 2024 · b1b5d42 · b1b5d42
1 parent ff6a115
commit b1b5d42
Showing 1 changed file with 3 additions and 2 deletions.
diff --git a/src/main/java/org/carlmontrobotics/commands/ArmTeleop.java b/src/main/java/org/carlmontrobotics/commands/ArmTeleop.java
@@ -50,8 +50,9 @@ public void execute() {
     double deltaT = currTime - lastTime;
 
     double goalArmRad = goalState.position + speeds * deltaT; 
-    goalArmRad = MathUtil.clamp(goalArmRad, LOWER_ANGLE_LIMIT, UPPER_ANGLE_LIMIT);
-    goalState = new TrapezoidProfile.State(goalArmRad, armSubsystem.getArmPos()-ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD);
+    goalArmRad = MathUtil.clamp(goalArmRad, UPPER_ANGLE_LIMIT, LOWER_ANGLE_LIMIT);
+    goalArmRad = MathUtil.clamp(goalArmRad, armSubsystem.getArmPos()+ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD, armSubsystem.getArmPos()-ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD);
+    goalState = new TrapezoidProfile.State(goalArmRad, 0);
     armSubsystem.setArmTarget(goalState.position);
     lastTime = currTime;
   }