Initial implementation of autonomous states, bit or work on wrist control, misc.

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

@@ -59,6 +59,11 @@ public class Robot {
 
     }
 
+    public enum WristPosition {
+        UP,
+        DOWN
+    }
+
     public enum Status {
         OKAY,
         MONITORING,
@@ -74,6 +79,9 @@ public class Robot {
     private final double wheelRadius, wheelGearRatio, armGearRatio;
     private final int wheelTicksPerRevolution, armTicksPerRevolution;
 
+    private WristPosition wristTargetPosition, wristCurrentPosition;
+    private double wristPositionChangeTime, wristPositionChangeRequestTime;
+
     private boolean LEDStatusToggle = false;
     private double lastLEDStatusAnimationTime = 0;
 
@@ -94,6 +102,10 @@ public class Robot {
         armGearRatio = tuningConfig("arm_gear_ratio").value();
         armTicksPerRevolution = tuningConfig("arm_ticks_per_revolution").value();
 
+        wristTargetPosition = WristPosition.UP;
+        wristCurrentPosition = WristPosition.UP;
+        wristPositionChangeTime = 2500;
+
         // FIXME: Rename motors in configuration
         // Define hardware
         backLeftDrive = engine.hardwareMap.get(DcMotorEx.class, "left_drive");    // MOTOR PORT: ?
@@ -372,6 +384,14 @@ public class Robot {
             hub.clearBulkCache();
         }
 
+        if (!wristManuallyControlled && wristTargetPosition != wristCurrentPosition &&
+                System.currentTimeMillis() - wristPositionChangeRequestTime >= wristPositionChangeTime) {
+            wristPositionChangeRequestTime = System.currentTimeMillis();
+            wristCurrentPosition = wristTargetPosition;
+
+            wrist.setPower(0);
+        }
+
         if (getVoltage() < 9.75) {
             reportStatus(Status.DANGER);
             hardwareFault = true;
@@ -440,6 +460,68 @@ public class Robot {
         }
     }
 
+    public void armPosition(ArmPosition position) {
+        if (hardwareFault) {
+            return;
+        }
+
+        reportStatus(Robot.Status.WARNING);
+
+        switch (position) {
+            case COLLECT:
+                arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_collect").value()));
+                break;
+
+            case GROUND:
+                arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_ground").value()));
+                break;
+
+            case LOW:
+                arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_low").value()));
+                break;
+
+            case MEDIUM:
+                arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_medium").value()));
+                break;
+
+            case HIGH:
+                arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_high").value()));
+                break;
+
+            default:
+                throw new RuntimeException("Unexpected arm position!");
+        }
+    }
+
+    public void wristPosition(WristPosition position) {
+        wristPositionChangeRequestTime = System.currentTimeMillis();
+        wristManuallyControlled = false;
+
+        if (position == WristPosition.UP) {
+            wrist.setPower(tuningConfig("wrist_up_power").value());
+        } else {
+            wrist.setPower(tuningConfig("wrist_down_power").value());
+        }
+    }
+
+    public void gripperOpen() {
+        gripper.setPosition(tuningConfig("gripper_open_position").value());
+    }
+
+    public void gripperClosed() {
+        gripper.setPosition(tuningConfig("gripper_closed_position").value());
+    }
+
+    // Adapted from: https://github.com/gosu/gosu/blob/980d64de2ce52e4b16fdd5cb9c9e11c8bbb80671/src/Math.cpp#L38
+    public double angleDiff(double from, double to) {
+        double value = (to - from + 180) - 180;
+
+        int fmod = (int) Math.floor(value - 0.0 / 360.0 - 0.0);
+        double result = (value - 0.0) - fmod * (360.0 - 0.0);
+
+        return result < 0 ? result + 360.0 : result + 0.0;
+    }
+
     public Status getStatus() { return status; }
 
     public double getRadius() { return radius; }

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

@@ -7,15 +7,47 @@ public class Arm extends CyberarmState {
     private final Robot robot;
     private final String groupName, actionName;
 
+    private final double targetVelocity, timeInMS;
+    private final int tolerance, targetPosition;
+    private final boolean stateDisabled;
+
     public Arm(Robot robot, String groupName, String actionName) {
         this.robot = robot;
         this.groupName = groupName;
         this.actionName = actionName;
+
+        targetVelocity = robot.angleToTicks(
+                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();
+
+        stateDisabled = !robot.getConfiguration().action(groupName, actionName).enabled;
+    }
+
+    @Override
+    public void start() {
+        robot.arm.setTargetPosition(targetPosition);
+        robot.arm.setTargetPositionTolerance(tolerance);
+        robot.arm.setVelocity(targetVelocity);
     }
 
     @Override
     public void exec() {
-        // FIXME: NO OP
+        if (stateDisabled) {
-        setHasFinished(true);
+            setHasFinished(true);
+
+            return;
+        }
+
+        if (runTime() >= timeInMS) {
+            stop();
+
+            setHasFinished(true);
+
+            return;
+        }
     }
 }

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

@@ -7,15 +7,49 @@ public class Gripper extends CyberarmState {
     private final Robot robot;
     private final String groupName, actionName;
 
+    private final boolean positionManually, stateDisabled;
+    private final String positionLookupLabel;
+    private final double manualPosition, timeInMS, position;
+
     public Gripper(Robot robot, String groupName, String actionName) {
         this.robot = robot;
         this.groupName = groupName;
         this.actionName = actionName;
+
+        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.getConfiguration().variable(groupName, actionName, "manualPosition").value();
+
+        stateDisabled = !robot.getConfiguration().action(groupName, actionName).enabled;
+
+        if (positionManually) {
+            position = manualPosition;
+        } else {
+            position = robot.tuningConfig(positionLookupLabel).value();
+        }
+    }
+
+    @Override
+    public void start() {
+        robot.gripper.setPosition(position);
     }
 
     @Override
     public void exec() {
-        // FIXME: NO OP
+        if (stateDisabled) {
-        setHasFinished(true);
+            setHasFinished(true);
+
+            return;
+        }
+
+        if (runTime() >= timeInMS) {
+            stop();
+
+            setHasFinished(true);
+
+            return;
+        }
     }
 }

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

@@ -108,7 +108,7 @@ public class Move extends CyberarmState {
         double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
 
 
-        double frontLeftPower = 0, frontRightPower = 0, backLeftPower = 0 , backRightPower = 0;
+        double frontLeftPower, frontRightPower, backLeftPower, backRightPower;
 
         double heading = robot.heading();
         double rotX = x * Math.cos(heading) - y * Math.sin(heading);

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

@@ -1,21 +1,101 @@
 package org.timecrafters.minibots.cyberarm.chiron.states.autonomous;
 
 import org.cyberarm.engine.V2.CyberarmState;
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
 import org.timecrafters.minibots.cyberarm.chiron.Robot;
 
 public class Rotate extends CyberarmState {
     private final Robot robot;
     private final String groupName, actionName;
 
+    private final double timeInMS, facing, targetFacing, targetVelocity, toleranceInDegrees;
+    private final boolean stateDisabled, useShortestRotation, rotateRight;
+
     public Rotate(Robot robot, String groupName, String actionName) {
         this.robot = robot;
         this.groupName = groupName;
         this.actionName = actionName;
+
+        timeInMS = robot.getConfiguration().variable(groupName, actionName, "timeInMS").value();
+
+        targetFacing = robot.getConfiguration().variable(groupName, actionName, "targetFacing").value();
+        useShortestRotation = robot.getConfiguration().variable(groupName, actionName, "useShortestRotation").value();
+        rotateRight = robot.getConfiguration().variable(groupName, actionName, "rotateRight").value();
+        toleranceInDegrees = robot.getConfiguration().variable(groupName, actionName, "toleranceInDegrees").value();
+        targetVelocity = robot.unitToTicks(DistanceUnit.INCH,
+                robot.getConfiguration().variable(groupName, actionName, "targetVelocityInInches").value());
+
+        stateDisabled = !robot.getConfiguration().action(groupName, actionName).enabled;
+
+        facing = (robot.facing() + targetFacing + 360.0) % 360.0;
     }
 
     @Override
     public void exec() {
-        // FIXME: NO OP
+        if (stateDisabled) {
-        setHasFinished(true);
+            stop();
+
+            setHasFinished(true);
+
+            return;
+        }
+
+        if (runTime() >= timeInMS) {
+            stop();
+
+            setHasFinished(true);
+
+            return;
+        }
+
+        double currentDegrees = robot.facing();
+        double diff = robot.angleDiff(facing, currentDegrees);
+
+        if (Math.abs(diff) <= toleranceInDegrees) {
+            stop();
+
+            setHasFinished(true);
+
+            return;
+        }
+
+        if (useShortestRotation) {
+            if (diff < 0) {
+                rotateLeft();
+            } else {
+                rotateRight();
+            }
+        } else {
+            if (rotateRight) {
+                rotateRight();
+            } else {
+                rotateLeft();
+            }
+        }
+    }
+
+    private void rotateLeft() {
+        rotate(-1);
+    }
+
+    private void rotateRight() {
+        rotate(1);
+    }
+
+    private void rotate(double multiplier) {
+        robot.frontRightDrive.setVelocity(targetVelocity * multiplier);
+        robot.backLeftDrive.setVelocity(targetVelocity * multiplier);
+
+        robot.backRightDrive.setVelocity(targetVelocity * multiplier);
+        robot.frontLeftDrive.setVelocity(targetVelocity * multiplier);
+    }
+
+    @Override
+    public void stop() {
+        robot.frontLeftDrive.setVelocity(0);
+        robot.frontRightDrive.setVelocity(0);
+
+        robot.backLeftDrive.setVelocity(0);
+        robot.backRightDrive.setVelocity(0);
     }
 }

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

@@ -13,6 +13,11 @@ public class SignalProcessor extends CyberarmState {
         this.actionName = actionName;
     }
 
+    @Override
+    public void start() {
+        engine.blackboardSet("parking_position", System.currentTimeMillis() % 3);
+    }
+
     @Override
     public void exec() {
         // FIXME: NO OP

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

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

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/ArmDriverControl.java

@@ -58,11 +58,10 @@ public class ArmDriverControl extends CyberarmState {
         double stepInterval = robot.tuningConfig("arm_manual_step_interval").value();
         int stepSize = robot.tuningConfig("arm_manual_step_size").value();
 
-        if ((controller.left_trigger > 0 || controller.right_trigger > 0) && runTime() - lastWristManualControlTime >= stepInterval) {
+        if ((controller.left_trigger > 0 || controller.right_trigger > 0) && runTime() - lastArmManualControlTime >= stepInterval) {
-            lastWristManualControlTime = runTime();
+            lastArmManualControlTime = runTime();
 
             if (controller.left_trigger > 0) { // Arm DOWN
-                // robot.arm.setVelocity(5, AngleUnit.DEGREES);
                 robot.arm.setTargetPosition(robot.arm.getCurrentPosition() - stepSize);
 
             } else if (controller.right_trigger > 0) { // Arm UP
@@ -150,47 +149,6 @@ public class ArmDriverControl extends CyberarmState {
         }
     }
 
-    private void armPosition(Robot.ArmPosition position) {
-        if (robot.hardwareFault) {
-            return;
-        }
-
-        robot.reportStatus(Robot.Status.WARNING);
-
-        switch (position) {
-            case COLLECT:
-                robot.arm.setTargetPosition(robot.angleToTicks(robot.tuningConfig("arm_position_angle_collect").value()));
-                break;
-
-            case GROUND:
-                robot.arm.setTargetPosition(robot.angleToTicks(robot.tuningConfig("arm_position_angle_ground").value()));
-                break;
-
-            case LOW:
-                robot.arm.setTargetPosition(robot.angleToTicks(robot.tuningConfig("arm_position_angle_low").value()));
-                break;
-
-            case MEDIUM:
-                robot.arm.setTargetPosition(robot.angleToTicks(robot.tuningConfig("arm_position_angle_medium").value()));
-                break;
-
-            case HIGH:
-                robot.arm.setTargetPosition(robot.angleToTicks(robot.tuningConfig("arm_position_angle_high").value()));
-                break;
-
-            default:
-                throw new RuntimeException("Unexpected arm position!");
-        }
-    }
-
-    private void gripperOpen() {
-        robot.gripper.setPosition(robot.tuningConfig("gripper_open_position").value());
-    }
-
-    private void gripperClosed() {
-        robot.gripper.setPosition(robot.tuningConfig("gripper_closed_position").value());
-    }
-
     @Override
     public void buttonDown(Gamepad gamepad, String button) {
         // Swap controlling gamepad
@@ -206,31 +164,32 @@ public class ArmDriverControl extends CyberarmState {
         if (button.equals("left_bumper")) {
             gripperReleaseTriggeredTime = runTime();
         } else if (button.equals("right_bumper")) {
-            gripperClosed();
+            robot.gripperClosed();
         }
 
         // Wrist Control
-        if (button.equals("dpad_down")) {
+        if (button.equals("dpad_up")) {
-            robot.wristManuallyControlled = false;
+            robot.wristPosition(Robot.WristPosition.UP);
-
-            robot.wrist.setPower(robot.tuningConfig("wrist_up_power").value());
         }
 
-        if (button.equals("dpad_up")) {
+        if (button.equals("dpad_down")) {
-            robot.wristManuallyControlled = false;
+            robot.wristPosition(Robot.WristPosition.DOWN);
-
-            robot.wrist.setPower(robot.tuningConfig("wrist_down_power").value());
         }
 
         // Automatic Arm Control
-        if (button.equals("a")) {
+        switch (button) {
-            armPosition(Robot.ArmPosition.COLLECT);
+            case "a":
-        } else if (button.equals("x")) {
+                robot.armPosition(Robot.ArmPosition.COLLECT);
-            armPosition(Robot.ArmPosition.GROUND);
+                break;
-        } else if (button.equals("b")) {
+            case "x":
-            armPosition(Robot.ArmPosition.LOW);
+                robot.armPosition(Robot.ArmPosition.GROUND);
-        } else if (button.equals("y")) {
+                break;
-            armPosition(Robot.ArmPosition.MEDIUM);
+            case "b":
+                robot.armPosition(Robot.ArmPosition.LOW);
+                break;
+            case "y":
+                robot.armPosition(Robot.ArmPosition.MEDIUM);
+                break;
         }
     }
 
@@ -242,7 +201,7 @@ public class ArmDriverControl extends CyberarmState {
 
         // Gripper Control - Require confirmation before opening gripper
         if (button.equals("left_bumper") && runTime() - gripperReleaseTriggeredTime >= gripperOpenConfirmationDelay) {
-            gripperOpen();
+            robot.gripperOpen();
         }
     }
 }