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

2025-12-15 16:42:35 +00:00 · 2023-01-25 19:42:45 -06:00
parent 6f62886453
commit 578ab45327
8 changed files with 315 additions and 69 deletions
--- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java
+++ b/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; }
--- 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
@@ -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
-        setHasFinished(true);
+        if (stateDisabled) {
+            setHasFinished(true);
+
+            return;
+        }
+
+        if (runTime() >= timeInMS) {
+            stop();
+
+            setHasFinished(true);
+
+            return;
+        }
    }
 }
--- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Gripper.java
+++ b/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
-        setHasFinished(true);
+        if (stateDisabled) {
+            setHasFinished(true);
+
+            return;
+        }
+
+        if (runTime() >= timeInMS) {
+            stop();
+
+            setHasFinished(true);
+
+            return;
+        }
    }
 }
--- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Move.java
+++ b/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);
--- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Rotate.java
+++ b/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
-        setHasFinished(true);
+        if (stateDisabled) {
+            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);
    }
 }
--- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/SignalProcessor.java
+++ b/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
--- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Wrist.java
+++ b/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);
+    }
+}
--- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/ArmDriverControl.java
+++ b/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) {
-            lastWristManualControlTime = runTime();
+        if ((controller.left_trigger > 0 || controller.right_trigger > 0) && runTime() - lastArmManualControlTime >= stepInterval) {
+            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")) {
-            robot.wristManuallyControlled = false;
-
-            robot.wrist.setPower(robot.tuningConfig("wrist_up_power").value());
+        if (button.equals("dpad_up")) {
+            robot.wristPosition(Robot.WristPosition.UP);
        }

-        if (button.equals("dpad_up")) {
-            robot.wristManuallyControlled = false;
-
-            robot.wrist.setPower(robot.tuningConfig("wrist_down_power").value());
+        if (button.equals("dpad_down")) {
+            robot.wristPosition(Robot.WristPosition.DOWN);
        }

        // Automatic Arm Control
-        if (button.equals("a")) {
-            armPosition(Robot.ArmPosition.COLLECT);
-        } else if (button.equals("x")) {
-            armPosition(Robot.ArmPosition.GROUND);
-        } else if (button.equals("b")) {
-            armPosition(Robot.ArmPosition.LOW);
-        } else if (button.equals("y")) {
-            armPosition(Robot.ArmPosition.MEDIUM);
+        switch (button) {
+            case "a":
+                robot.armPosition(Robot.ArmPosition.COLLECT);
+                break;
+            case "x":
+                robot.armPosition(Robot.ArmPosition.GROUND);
+                break;
+            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();
        }
    }
 }