Rework Arm state to be able to use 'tuning' variables for arm angle

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java

@@ -563,6 +563,11 @@ public class Robot {
         return (fmod < 0 ? fmod + 360.0 : fmod +  0.0) - 180;
     }
 
+    public double lerp(double min, double max, double t)
+    {
+        return min + (max - min) * t;
+    }
+
     public Status getStatus() { return status; }
 
     public double getRadius() { return radius; }

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Arm.java

@@ -8,8 +8,9 @@ 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, position, manualPosition;
-    private final boolean stateDisabled;
+    private final boolean positionManually, stateDisabled;
+    private final String positionLookupLabel;
 
     public Arm(Robot robot, String groupName, String actionName) {
         this.robot = robot;
@@ -20,16 +21,27 @@ public class Arm extends CyberarmState {
                 robot.getConfiguration().variable(groupName, actionName, "targetVelocityInDegreesPerSecond").value());
         tolerance = robot.angleToTicks(
                 robot.getConfiguration().variable(groupName, actionName, "toleranceInDegrees").value());
-        targetPosition = robot.angleToTicks(
-                robot.getConfiguration().variable(groupName, actionName, "targetAngle").value());
         timeInMS = robot.getConfiguration().variable(groupName, actionName, "timeInMS").value();
 
+        positionLookupLabel = robot.getConfiguration().variable(groupName, actionName, "positionLookupLabel").value();
+        positionManually = robot.getConfiguration().variable(groupName, actionName, "positionManually").value();
+        manualPosition = robot.angleToTicks(
+                robot.getConfiguration().variable(groupName, actionName, "manualTargetAngle").value());
+
         stateDisabled = !robot.getConfiguration().action(groupName, actionName).enabled;
+
+        halfTolerance = Math.round(tolerance / 2.0f);
+
+        if (positionManually) {
+            position = manualPosition;
+        } else {
+            position = robot.angleToTicks(robot.tuningConfig(positionLookupLabel).value());
+        }
     }
 
     @Override
     public void start() {
-        robot.arm.setTargetPosition(targetPosition);
+        robot.arm.setTargetPosition(position);
         robot.arm.setTargetPositionTolerance(tolerance);
         robot.arm.setVelocity(targetVelocity);
     }
@@ -49,5 +61,10 @@ public class Arm extends CyberarmState {
 
             return;
         }
+
+        int currentPosition = robot.arm.getCurrentPosition();
+        if (robot.isBetween(currentPosition, currentPosition - halfTolerance, currentPosition + halfTolerance)) {
+            setHasFinished(true);
+        }
     }
 }

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Move.java

@@ -1,5 +1,7 @@
 package org.timecrafters.minibots.cyberarm.chiron.states.autonomous;
 
+import com.qualcomm.robotcore.util.Range;
+
 import org.cyberarm.engine.V2.CyberarmState;
 import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
 import org.timecrafters.minibots.cyberarm.chiron.Robot;
@@ -72,8 +74,7 @@ public class Move extends CyberarmState {
     }
 
     private void moveDirectional(double travelledDistance) {
-        double velocity = targetVelocity;
+        double velocity;
-        double easeVelocity;
         double ratio = 1.0;
 
         if (Math.abs(travelledDistance) < easeInDistance) {
@@ -82,10 +83,9 @@ public class Move extends CyberarmState {
             ratio = (targetDistance - Math.abs(travelledDistance)) / easeOutDistance;
         }
 
-        easeVelocity = targetVelocity * ratio;
+        ratio = Range.clip(ratio, 0.0, 1.0);
 
-        if (easeVelocity < minimumVelocity) { easeVelocity = minimumVelocity; }
+        velocity = robot.lerp(minimumVelocity, targetVelocity, ratio);
-        if (easeVelocity < targetVelocity) { velocity = easeVelocity; }
 
         if (targetDistance > travelledDistance) {
             robot.frontRightDrive.setVelocity(-velocity);

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Wait.java

@@ -0,0 +1,39 @@
+package org.timecrafters.minibots.cyberarm.chiron.states.autonomous;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.minibots.cyberarm.chiron.Robot;
+
+public class Wait extends CyberarmState {
+    private final Robot robot;
+    private final String groupName, actionName;
+
+    private final double timeInMS;
+    private final boolean stateDisabled;
+
+    public Wait(Robot robot, String groupName, String actionName) {
+        this.robot = robot;
+        this.groupName = groupName;
+        this.actionName = actionName;
+
+        timeInMS = robot.getConfiguration().variable(groupName, actionName, "timeInMS").value();
+
+        stateDisabled = !robot.getConfiguration().action(groupName, actionName).enabled;
+    }
+
+    @Override
+    public void exec() {
+        if (stateDisabled) {
+            setHasFinished(true);
+
+            return;
+        }
+
+        if (runTime() >= timeInMS) {
+            stop();
+
+            setHasFinished(true);
+
+            return;
+        }
+    }
+}