From 4b04dc27998af41e6542f0bff0c7fe614978476b Mon Sep 17 00:00:00 2001 From: Cyberarm Date: Sun, 29 Jan 2023 17:47:57 -0600 Subject: [PATCH] Finish Arm state when Arm arrives --- .../minibots/cyberarm/chiron/states/autonomous/Arm.java | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Arm.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Arm.java index cb27629..2374cfe 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Arm.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Arm.java @@ -8,7 +8,7 @@ public class Arm extends CyberarmState { private final String groupName, actionName; private final double targetVelocity, timeInMS; - private final int tolerance, targetPosition; + private final int tolerance, halfTolerance, targetPosition; private final boolean stateDisabled; public Arm(Robot robot, String groupName, String actionName) { @@ -25,6 +25,8 @@ public class Arm extends CyberarmState { timeInMS = robot.getConfiguration().variable(groupName, actionName, "timeInMS").value(); stateDisabled = !robot.getConfiguration().action(groupName, actionName).enabled; + + halfTolerance = Math.round(tolerance / 2.0f); } @Override @@ -49,5 +51,10 @@ public class Arm extends CyberarmState { return; } + + int position = robot.arm.getCurrentPosition(); + if (robot.isBetween(position, position - halfTolerance, position + halfTolerance)) { + setHasFinished(true); + } } }