From 578ab4532716e6cc7a071f11bd0c6ff1430e6822 Mon Sep 17 00:00:00 2001 From: cyberarm Date: Wed, 25 Jan 2023 19:42:45 -0600 Subject: [PATCH] Initial implementation of autonomous states, bit or work on wrist control, misc. --- .../minibots/cyberarm/chiron/Robot.java | 82 ++++++++++++++++++ .../chiron/states/autonomous/Arm.java | 36 +++++++- .../chiron/states/autonomous/Gripper.java | 38 ++++++++- .../chiron/states/autonomous/Move.java | 2 +- .../chiron/states/autonomous/Rotate.java | 84 ++++++++++++++++++- .../states/autonomous/SignalProcessor.java | 5 ++ .../chiron/states/autonomous/Wrist.java | 54 ++++++++++++ .../states/teleop/ArmDriverControl.java | 83 +++++------------- 8 files changed, 315 insertions(+), 69 deletions(-) create mode 100644 TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Wrist.java diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java index f375065..48b26dd 100644 --- 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; } 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 da30682..cb27629 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 @@ -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; + } } } diff --git 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 index 83c1e2a..e94199b 100644 --- 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; + } } } diff --git 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 index fda2ea3..3b463f2 100644 --- 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); diff --git 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 index 61cd6e1..b48bb85 100644 --- 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); } } diff --git 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 index fdcce2b..5965d8f 100644 --- 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 diff --git 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 new file mode 100644 index 0000000..9d8e216 --- /dev/null +++ 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); + } +} diff --git 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 index 937e852..b088fac 100644 --- 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(); } } }