From b023d883f1fb8fbcb68d5af057a6e05da3c0fb9f Mon Sep 17 00:00:00 2001 From: Sodi Date: Thu, 29 Dec 2022 19:16:37 -0600 Subject: [PATCH 1/7] Adding run-to on arm heights and telemetry, finished all 4 junction heights, overrides, etc. --- .../TeleOp/states/PhoenixTeleOPState.java | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java index 7ebef17..df407b7 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java +++ b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java @@ -304,7 +304,7 @@ public class PhoenixTeleOPState extends CyberarmState { } } - if (/*robot.LowRiserLeft.getPosition() < 0.75 &&*/ robot.HighRiserLeft.getPosition() > 0.85 && downSensor() < 840) { + if (robot.LowRiserLeft.getPosition() < 0.75 && robot.HighRiserLeft.getPosition() > 0.85) { if (System.currentTimeMillis() - lastStepTime >= 150) { lastStepTime = System.currentTimeMillis(); robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05); @@ -516,18 +516,13 @@ public class PhoenixTeleOPState extends CyberarmState { } } public double downSensor() { - double Distance, Distance_1, Distance_2, Distance_3, Distance_4, Distance_5, Distance_6, Distance_7, Distance_8, Distance_9, Distance_10; + double Distance, Distance_1, Distance_2, Distance_3, Distance_4, Distance_5; Distance_1 = robot.downSensor.getDistance(DistanceUnit.MM); Distance_2 = robot.downSensor.getDistance(DistanceUnit.MM); Distance_3 = robot.downSensor.getDistance(DistanceUnit.MM); Distance_4 = robot.downSensor.getDistance(DistanceUnit.MM); Distance_5 = robot.downSensor.getDistance(DistanceUnit.MM); -// Distance_6 = robot.downSensor.getDistance(DistanceUnit.MM); -// Distance_7 = robot.downSensor.getDistance(DistanceUnit.MM); -// Distance_8 = robot.downSensor.getDistance(DistanceUnit.MM); -// Distance_9 = robot.downSensor.getDistance(DistanceUnit.MM); -// Distance_10 = robot.downSensor.getDistance(DistanceUnit.MM); - Distance = (Distance_1 + Distance_2 + Distance_3 + Distance_4 + Distance_5/* + Distance_6 + Distance_7 + Distance_8 + Distance_9 + Distance_10*/)/5; + Distance = (Distance_1 + Distance_2 + Distance_3 + Distance_4 + Distance_5)/5; return Distance; } From 1ef383de18b4a55cbb27ebd2b3ff5ce7393cea56 Mon Sep 17 00:00:00 2001 From: Sodi Date: Fri, 30 Dec 2022 09:46:16 -0600 Subject: [PATCH 2/7] Changing dpad left/right to 90/-90 degrees instead of 45/-45. --- .../TeleOp/states/PhoenixTeleOPState.java | 158 ++++++++---------- 1 file changed, 72 insertions(+), 86 deletions(-) diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java index df407b7..12abc2a 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java +++ b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java @@ -123,10 +123,64 @@ public class PhoenixTeleOPState extends CyberarmState { Math.abs(engine.gamepad1.left_stick_y) < 0.1 && Math.abs(engine.gamepad1.right_stick_y) < 0.1) { drivePower = 0; - robot.backLeftDrive.setPower(-drivePower); - robot.backRightDrive.setPower(-drivePower); - robot.frontLeftDrive.setPower(-drivePower); - robot.frontRightDrive.setPower(-drivePower); + robot.backLeftDrive.setPower(drivePower); + robot.backRightDrive.setPower(drivePower); + robot.frontLeftDrive.setPower(drivePower); + robot.frontRightDrive.setPower(drivePower); + } + + if (engine.gamepad1.dpad_right) { + RobotRotation = robot.imu.getAngularOrientation().firstAngle; + RotationTarget = 90; + CalculateDeltaRotation(); + if (RobotRotation > -90 && RobotRotation < 89) {//CCW + drivePower = (-0.95 * DeltaRotation / 180) - MinimalPower; + robot.backLeftDrive.setPower(drivePower); + robot.backRightDrive.setPower(-drivePower); + robot.frontLeftDrive.setPower(drivePower); + robot.frontRightDrive.setPower(-drivePower); + } + if (RobotRotation > 91 || RobotRotation < -90) {//CW + drivePower = (0.95 * DeltaRotation / 180) + MinimalPower; + robot.backLeftDrive.setPower(drivePower); + robot.backRightDrive.setPower(-drivePower); + robot.frontLeftDrive.setPower(drivePower); + robot.frontRightDrive.setPower(-drivePower); + } + if (RobotRotation < 91 && RobotRotation > 89) { + drivePower = 0; + robot.backLeftDrive.setPower(drivePower); + robot.backRightDrive.setPower(drivePower); + robot.frontLeftDrive.setPower(drivePower); + robot.frontRightDrive.setPower(drivePower); + } + } + + if (engine.gamepad1.dpad_left) { + RobotRotation = robot.imu.getAngularOrientation().firstAngle; + RotationTarget = -90; + CalculateDeltaRotation(); + if (RobotRotation > -89 && RobotRotation <= 90) {//CW + drivePower = (0.95 * DeltaRotation / 180) - MinimalPower; + robot.backLeftDrive.setPower(drivePower); + robot.backRightDrive.setPower(-drivePower); + robot.frontLeftDrive.setPower(drivePower); + robot.frontRightDrive.setPower(-drivePower); + } + if (RobotRotation > 90 || RobotRotation < -91) {//CCW + drivePower = (-0.95 * DeltaRotation / 180) + MinimalPower; + robot.backLeftDrive.setPower(drivePower); + robot.backRightDrive.setPower(-drivePower); + robot.frontLeftDrive.setPower(drivePower); + robot.frontRightDrive.setPower(-drivePower); + } + if (RobotRotation < 91 && RobotRotation > 89) { + drivePower = 0; + robot.backLeftDrive.setPower(drivePower); + robot.backRightDrive.setPower(drivePower); + robot.frontLeftDrive.setPower(drivePower); + robot.frontRightDrive.setPower(drivePower); + } } if (engine.gamepad1.y) { @@ -134,13 +188,13 @@ public class PhoenixTeleOPState extends CyberarmState { RotationTarget = 180; CalculateDeltaRotation(); if (RobotRotation < 0 && RobotRotation > -179) { - drivePower = (1 * DeltaRotation / 180) + MinimalPower; + drivePower = (0.95 * DeltaRotation / 180) + MinimalPower; robot.backLeftDrive.setPower(drivePower); robot.backRightDrive.setPower(-drivePower); robot.frontLeftDrive.setPower(drivePower); robot.frontRightDrive.setPower(-drivePower); } else if (RobotRotation >= 0) { - drivePower = (-1 * DeltaRotation / 180) - MinimalPower; + drivePower = (-0.95 * DeltaRotation / 180) - MinimalPower; robot.backLeftDrive.setPower(drivePower); robot.backRightDrive.setPower(-drivePower); robot.frontLeftDrive.setPower(drivePower); @@ -159,14 +213,14 @@ public class PhoenixTeleOPState extends CyberarmState { RotationTarget = 0; CalculateDeltaRotation(); if (RobotRotation < -1) { - drivePower = (-1 * DeltaRotation / 180) - MinimalPower; + drivePower = (-0.95 * DeltaRotation / 180) - MinimalPower; robot.backLeftDrive.setPower(drivePower); robot.backRightDrive.setPower(-drivePower); robot.frontLeftDrive.setPower(drivePower); robot.frontRightDrive.setPower(-drivePower); } if (RobotRotation > 1) { - drivePower = (1 * DeltaRotation / 180) + MinimalPower; + drivePower = (0.95 * DeltaRotation / 180) + MinimalPower; robot.backLeftDrive.setPower(drivePower); robot.backRightDrive.setPower(-drivePower); robot.frontLeftDrive.setPower(drivePower); @@ -186,14 +240,14 @@ public class PhoenixTeleOPState extends CyberarmState { RotationTarget = -45; CalculateDeltaRotation(); if (RobotRotation < -46 || RobotRotation > 135) {//CCW - drivePower = (-1 * DeltaRotation / 180) - MinimalPower; + drivePower = (-0.95 * DeltaRotation / 180) - MinimalPower; robot.backLeftDrive.setPower(drivePower); robot.backRightDrive.setPower(-drivePower); robot.frontLeftDrive.setPower(drivePower); robot.frontRightDrive.setPower(-drivePower); } if (RobotRotation > -44 && RobotRotation < 135) {//CW - drivePower = (1 * DeltaRotation / 180) + MinimalPower; + drivePower = (0.95 * DeltaRotation / 180) + MinimalPower; robot.backLeftDrive.setPower(drivePower); robot.backRightDrive.setPower(-drivePower); robot.frontLeftDrive.setPower(drivePower); @@ -202,36 +256,9 @@ public class PhoenixTeleOPState extends CyberarmState { if (RobotRotation > -46 && RobotRotation < -44) { drivePower = 0; robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(-drivePower); + robot.backRightDrive.setPower(drivePower); robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(-drivePower); - } - } - - if (engine.gamepad1.dpad_right) { - RobotRotation = robot.imu.getAngularOrientation().firstAngle; - RotationTarget = 45; - CalculateDeltaRotation(); - if (RobotRotation > -135 && RobotRotation < 44) {//CCW - drivePower = (-1 * DeltaRotation / 180) - MinimalPower; - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(-drivePower); - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(-drivePower); - } - if (RobotRotation < -135 || RobotRotation < 46) {//CW - drivePower = (1 * DeltaRotation / 180) + MinimalPower; - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(-drivePower); - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(-drivePower); - } - if (RobotRotation < 46 && RobotRotation > 44) { - drivePower = 0; - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(-drivePower); - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(-drivePower); + robot.frontRightDrive.setPower(drivePower); } } @@ -240,14 +267,14 @@ public class PhoenixTeleOPState extends CyberarmState { RotationTarget = 45; CalculateDeltaRotation(); if (RobotRotation > -135 && RobotRotation < 44) {//CCW - drivePower = (-1 * DeltaRotation / 180) - MinimalPower; + drivePower = (-0.95 * DeltaRotation / 180) - MinimalPower; robot.backLeftDrive.setPower(drivePower); robot.backRightDrive.setPower(-drivePower); robot.frontLeftDrive.setPower(drivePower); robot.frontRightDrive.setPower(-drivePower); } if (RobotRotation < -135 || RobotRotation > 46) {//CW - drivePower = (1 * DeltaRotation / 180) + MinimalPower; + drivePower = (0.95 * DeltaRotation / 180) + MinimalPower; robot.backLeftDrive.setPower(drivePower); robot.backRightDrive.setPower(-drivePower); robot.frontLeftDrive.setPower(drivePower); @@ -256,9 +283,9 @@ public class PhoenixTeleOPState extends CyberarmState { if (RobotRotation < 46 && RobotRotation > 44) { drivePower = 0; robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(-drivePower); + robot.backRightDrive.setPower(drivePower); robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(-drivePower); + robot.frontRightDrive.setPower(drivePower); } } @@ -331,49 +358,8 @@ public class PhoenixTeleOPState extends CyberarmState { } }//end of a -// if (engine.gamepad2.back) { -// robot.backLeftDrive.setPower(1); -// robot.backRightDrive.setPower(1); -// robot.frontLeftDrive.setPower(1); -// robot.frontRightDrive.setPower(1); -// if (System.currentTimeMillis() - lastStepTime >= 1500) { -// robot.backLeftDrive.setPower(0); -// robot.backRightDrive.setPower(0); -// robot.frontLeftDrive.setPower(0); -// robot.frontRightDrive.setPower(0); -// } -// if (System.currentTimeMillis() - lastStepTime >= 150) { -// if (robot.HighRiserLeft.getPosition() < 1) { -// if (System.currentTimeMillis() - lastStepTime >= 150) { -// lastStepTime = System.currentTimeMillis(); -// robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05); -// robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05); -// } -// } -// } -// if (System.currentTimeMillis() - lastStepTime >= 150) { -// if (robot.LowRiserLeft.getPosition() < 1 && robot.HighRiserLeft.getPosition() == 1) { -// if (System.currentTimeMillis() - lastStepTime >= 150) { -// lastStepTime = System.currentTimeMillis(); -// robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05); -// robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05); -// } -// } -// } -// if (System.currentTimeMillis() >= 250) { -// robot.backLeftDrive.setPower(1); -// robot.backRightDrive.setPower(1); -// robot.frontLeftDrive.setPower(1); -// robot.frontRightDrive.setPower(1); -// if (System.currentTimeMillis() - lastStepTime >= 250) { -// robot.backLeftDrive.setPower(0); -// robot.backRightDrive.setPower(0); -// robot.frontLeftDrive.setPower(0); -// robot.frontRightDrive.setPower(0); -// } -// } -// -// } + //i feel like removing that was a stupidly impulsive move but i gave up on not being stupidly + // impulsive a long time ago lol. Besides, when have we even used that function? It got replaced. if (engine.gamepad2.a) { OCD = 1; From 0250394f567dac4757b1ff8221538f6e2246ad3c Mon Sep 17 00:00:00 2001 From: Cyberarm Date: Sat, 31 Dec 2022 10:58:25 -0600 Subject: [PATCH 3/7] Made blackboard system a first-class member of CyberarmEngine, added support for background tasks (queue-less states) to CyberarmEngine, added helper method for adding parallel states to last added state of engine. --- .../cyberarm/engine/V2/CyberarmEngine.java | 111 +++++++++++++++++- .../Engines/LeftSideAutonomousEngine.java | 2 +- .../RightFourConeAutonomousEngine.java | 2 +- .../Engines/RightSideAutonomousEngine.java | 2 +- .../Autonomous/States/ConeIdentification.java | 14 +-- .../States/DriverParkPlaceState.java | 2 +- .../Autonomous/States/PathDecision.java | 2 +- 7 files changed, 118 insertions(+), 17 deletions(-) diff --git a/TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmEngine.java b/TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmEngine.java index 2a8307e..e78ba8d 100644 --- a/TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmEngine.java +++ b/TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmEngine.java @@ -11,9 +11,15 @@ import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Act import java.lang.reflect.Constructor; import java.lang.reflect.InvocationTargetException; import java.util.HashMap; +import java.util.concurrent.ConcurrentHashMap; import java.util.concurrent.CopyOnWriteArrayList; /** + * CyberarmEngine Version 3.0 | December 31st 2022 + * After a few years of use, it's safe to say this implementation is stable and reasonably feature complete. + * * Added support for background tasks that run unqueued for the duration of the op mode unless stopped. + * * Added thread-safe 'blackboard' for storing bits that need to be easily shared between states/tasks. + * * CyberarmEngine Version 2.0 | October 26th 2018 * AN Experimental reimplementation of GoldfishPi's original Engine system. * Designed to be easily maintainable, extensible, and easy to understand. @@ -23,9 +29,12 @@ public abstract class CyberarmEngine extends OpMode { public static CyberarmEngine instance; //Array To Hold States final private CopyOnWriteArrayList cyberarmStates = new CopyOnWriteArrayList<>(); - public HashMap blackboard = new HashMap<>(); - private int activeStateIndex = 0; - private boolean isRunning; + // Array to Hold Tasks + final private CopyOnWriteArrayList backgroundTasks = new CopyOnWriteArrayList<>(); + // HashMap to store data for States and Tasks + private ConcurrentHashMap blackboard = new ConcurrentHashMap<>(); + private int activeStateIndex = 0; // Index of currently running state + private boolean isRunning; // Whether engine is running or not private final static String TAG = "PROGRAM.ENGINE"; public boolean showStateChildrenListInTelemetry = false; @@ -49,6 +58,11 @@ public abstract class CyberarmEngine extends OpMode { for (CyberarmState state: cyberarmStates) { initState(state); } + + // Background tasks + for (CyberarmState task : backgroundTasks) { + initState(task); + } } /** @@ -73,6 +87,11 @@ public abstract class CyberarmEngine extends OpMode { if (cyberarmStates.size() > 0) { runState(cyberarmStates.get(0)); } + + // Background tasks + for (CyberarmState task : backgroundTasks) { + runState(task); + } } /** @@ -95,7 +114,11 @@ public abstract class CyberarmEngine extends OpMode { } // Add telemetry to show currently running state - telemetry.addLine("Running state: " +state.getClass().getSimpleName() + ". State: " + activeStateIndex + " of " + (cyberarmStates.size()-1)); + telemetry.addLine( + "Running state: " +state.getClass().getSimpleName() + ". State: " + + (activeStateIndex + 1) + " of " + (cyberarmStates.size()) + + " (" + activeStateIndex + "/" + (cyberarmStates.size() - 1) + ")"); + if (showStateChildrenListInTelemetry && state.hasChildren()) { for(CyberarmState child: state.children) { telemetry.addLine(" Child: " + child.getClass().getSimpleName() + " [" + child.children.size() + "] grandchildren"); @@ -113,6 +136,11 @@ public abstract class CyberarmEngine extends OpMode { } else { stateTelemetry(state); + + // Background tasks + for (CyberarmState task : backgroundTasks) { + initState(task); + } } gamepadCheckerGamepad1.update(); @@ -128,7 +156,10 @@ public abstract class CyberarmEngine extends OpMode { stopState(state); } - + // Background tasks + for (CyberarmState task : backgroundTasks) { + stopState(task); + } } /** @@ -225,6 +256,76 @@ public abstract class CyberarmEngine extends OpMode { return state; } + /** + * Adds state to the most recently added top level state as a parallel state + * @param state State to add to last top level state + * @return CyberarmState + */ + public CyberarmState addParallelStateToLastState(CyberarmState state) { + CyberarmState parentState = cyberarmStates.get(cyberarmStates.size() - 1); + + Log.i(TAG, "Adding parallel cyberarmState "+ state.getClass() + " to parent state " + parentState.getClass()); + + parentState.addParallelState((state)); + + return state; + } + + /** + * Adds state as a background task that is run until the opmode stops + * background tasks are not queued, they are all started at once. + * @param state State to add to list + * @return CyberarmState + */ + public CyberarmState addTask(CyberarmState state) { + Log.i(TAG, "Adding task cyberarmState "+ state.getClass()); + + backgroundTasks.add(state); + + if (isRunning()) { + initState(state); + runState(state); + } + + return state; + } + + /** + * Retrieve value from blackboard + * @param key String to use to look up value + * @return Returns Object which should be autocast to the correct type + */ + @SuppressWarnings("unchecked") + public T blackboard_get(String key) { + return (T) blackboard.get(key); + } + + /** + * Set value of key to value + * @param key String + * @param value Object + * @return Returns T + */ + @SuppressWarnings("unchecked") + public T blackboard_set(String key, Object value) { + blackboard.put(key, value); + + return (T) value; + } + + /** + * Remove value from blackboard + * @param key String + * @param value Object + * @return Returns T + */ + @SuppressWarnings("unchecked") + public T blackboard_remove(String key, Object value) { + blackboard.remove(key); + + return (T) value; + } + private void buttonDownForStates(CyberarmState state, Gamepad gamepad, String button) { state.buttonDown(gamepad, button); diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftSideAutonomousEngine.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftSideAutonomousEngine.java index f6d78a9..5f69767 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftSideAutonomousEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftSideAutonomousEngine.java @@ -96,6 +96,6 @@ public class LeftSideAutonomousEngine extends CyberarmEngine { public void loop() { super.loop(); - telemetry.addData("BlackBoard Input", blackboard.get("parkPlace")); + telemetry.addData("BlackBoard Input", blackboard_get("parkPlace")); } } diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightFourConeAutonomousEngine.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightFourConeAutonomousEngine.java index 34dbdf9..0032c62 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightFourConeAutonomousEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightFourConeAutonomousEngine.java @@ -148,6 +148,6 @@ public class RightFourConeAutonomousEngine extends CyberarmEngine { public void loop() { super.loop(); - telemetry.addData("BlackBoard Input", blackboard.get("parkPlace")); + telemetry.addData("BlackBoard Input", blackboard_get("parkPlace")); } } diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightSideAutonomousEngine.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightSideAutonomousEngine.java index 249b0ed..c714223 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightSideAutonomousEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightSideAutonomousEngine.java @@ -97,6 +97,6 @@ public class RightSideAutonomousEngine extends CyberarmEngine { public void loop() { super.loop(); - telemetry.addData("BlackBoard Input", blackboard.get("parkPlace")); + telemetry.addData("BlackBoard Input", blackboard_get("parkPlace")); } } diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/ConeIdentification.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/ConeIdentification.java index 554034c..6bb512a 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/ConeIdentification.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/ConeIdentification.java @@ -21,7 +21,7 @@ public class ConeIdentification extends CyberarmState { @Override public void init() { - engine.blackboard.put("parkPlace", "1"); + engine.blackboard_set("parkPlace", "1"); robot.tfod.activate(); initTime = System.currentTimeMillis(); } @@ -50,11 +50,11 @@ public class ConeIdentification extends CyberarmState { engine.telemetry.addData("- Size (Width/Height)","%.0f / %.0f", width, height); if (recognition.getLabel().equals("#2")) { - engine.telemetry.addData("#2", engine.blackboard.put("parkPlace", "2")); + engine.telemetry.addData("#2", engine.blackboard_set("parkPlace", "2")); } else if (recognition.getLabel().equals("#3")) { - engine.telemetry.addData("#3",engine.blackboard.put("parkPlace", "3")); + engine.telemetry.addData("#3",engine.blackboard_set("parkPlace", "3")); } else { - engine.telemetry.addData("#1", engine.blackboard.put("parkPlace", "1")); + engine.telemetry.addData("#1", engine.blackboard_set("parkPlace", "1")); } } } @@ -90,12 +90,12 @@ public class ConeIdentification extends CyberarmState { bestConfidence = recognition.getConfidence(); if (recognition.getLabel().equals("2 Bulb")) { - engine.blackboard.put("parkPlace", "2"); + engine.blackboard_set("parkPlace", "2"); } else if (recognition.getLabel().equals("3 Panel")) { - engine.blackboard.put("parkPlace", "3"); + engine.blackboard_set("parkPlace", "3"); } else { - engine.blackboard.put("parkPlace", "1"); + engine.blackboard_set("parkPlace", "1"); } } } diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverParkPlaceState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverParkPlaceState.java index 816e91c..f1dd8cf 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverParkPlaceState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverParkPlaceState.java @@ -43,7 +43,7 @@ public class DriverParkPlaceState extends CyberarmState { setHasFinished(true); return; } - String placement = engine.blackboard.get("parkPlace"); + String placement = engine.blackboard_get("parkPlace"); if (placement != null) { if (!placement.equals(intendedPlacement)){ setHasFinished(true); diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/PathDecision.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/PathDecision.java index 051f527..483468d 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/PathDecision.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/PathDecision.java @@ -16,7 +16,7 @@ public class PathDecision extends CyberarmState { @Override public void exec() { - String placement = engine.blackboard.get("parkPlace"); + String placement = engine.blackboard_get("parkPlace"); setHasFinished(true); } From daf15a1b95a51d41d30fe74a8a509705100ae084 Mon Sep 17 00:00:00 2001 From: Sodi Date: Sat, 31 Dec 2022 11:12:06 -0600 Subject: [PATCH 4/7] Changing dpad left/right to 90/-90 degrees instead of 45/-45. --- .../TeleOp/states/PhoenixTeleOPState.java | 41 ++++++++++--------- 1 file changed, 21 insertions(+), 20 deletions(-) diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java index 12abc2a..9195d8f 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java +++ b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java @@ -20,7 +20,7 @@ public class PhoenixTeleOPState extends CyberarmState { private int CyclingArmUpAndDown = 0; private double RobotRotation; private double RotationTarget, DeltaRotation; - private double MinimalPower = 0.25; + private double MinimalPower = 0.25, topServoOffset = 0.05; private GamepadChecker gamepad1Checker, gamepad2Checker; private int OCD; @@ -67,11 +67,12 @@ public class PhoenixTeleOPState extends CyberarmState { robot.LowRiserRight.setDirection(Servo.Direction.REVERSE); robot.LowRiserLeft.setPosition(0.45); robot.LowRiserRight.setPosition(0.45); - robot.HighRiserLeft.setPosition(0.45); + robot.HighRiserLeft.setPosition(0.45 + topServoOffset); robot.HighRiserRight.setPosition(0.45); robot.OdometerEncoder.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); robot.OdometerEncoderLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + topServoOffset = robot.HighRiserRight.getPosition() + 0.05; gamepad1Checker = new GamepadChecker(engine, engine.gamepad1); gamepad2Checker = new GamepadChecker(engine, engine.gamepad2); OCD = 0; @@ -305,7 +306,7 @@ public class PhoenixTeleOPState extends CyberarmState { if (System.currentTimeMillis() - lastStepTime >= 150) { lastStepTime = System.currentTimeMillis(); OCD = 0; - robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05); + robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05 + topServoOffset); robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05); } } @@ -316,7 +317,7 @@ public class PhoenixTeleOPState extends CyberarmState { if (System.currentTimeMillis() - lastStepTime >= 150) { lastStepTime = System.currentTimeMillis(); OCD = 0; - robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05); + robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05 + topServoOffset); robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.05); } } @@ -326,7 +327,7 @@ public class PhoenixTeleOPState extends CyberarmState { if (robot.HighRiserLeft.getPosition() < 0.85) { if (System.currentTimeMillis() - lastStepTime >= 150) { lastStepTime = System.currentTimeMillis(); - robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05); + robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05 + topServoOffset); robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05); } } @@ -344,7 +345,7 @@ public class PhoenixTeleOPState extends CyberarmState { if (robot.HighRiserLeft.getPosition() > 0.45 && robot.LowRiserLeft.getPosition() < 0.5) { if (System.currentTimeMillis() - lastStepTime >= 150) { lastStepTime = System.currentTimeMillis(); - robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05); + robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05 + topServoOffset); robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.05); } } @@ -387,7 +388,7 @@ public class PhoenixTeleOPState extends CyberarmState { } else if (robot.LowRiserLeft.getPosition() <= 0.46 && robot.HighRiserLeft.getPosition() > 0.46) { if (System.currentTimeMillis() - lastStepTime >= 125) { lastStepTime = System.currentTimeMillis(); - robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05); + robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05 + topServoOffset); robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.05); } } else if (robot.LowRiserLeft.getPosition() <= 0.46 && robot.HighRiserLeft.getPosition() <= 0.46) { @@ -396,31 +397,31 @@ public class PhoenixTeleOPState extends CyberarmState { } if (OCD == 2) { //low junction - if (robot.LowRiserLeft.getPosition() > 0.46 && robot.HighRiserLeft.getPosition() > 0.65) { + if (robot.LowRiserLeft.getPosition() > 0.46) { if (System.currentTimeMillis() - lastStepTime >= 125) { lastStepTime = System.currentTimeMillis(); robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05); robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05); } - } // <-- both levels too high + } // <-- low level too high if (robot.LowRiserLeft.getPosition() <= 0.46 && robot.LowRiserLeft.getPosition() > 0.44 && robot.HighRiserLeft.getPosition() > 0.66) { if (System.currentTimeMillis() - lastStepTime >= 125) { lastStepTime = System.currentTimeMillis(); - robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05); + robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05 + topServoOffset); robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.05); } } // <-- top level too high - if (robot.LowRiserLeft.getPosition() < 0.44 && robot.HighRiserLeft.getPosition() < 0.64) { + if (robot.LowRiserLeft.getPosition() < 0.44) { if (System.currentTimeMillis() - lastStepTime >= 125) { lastStepTime = System.currentTimeMillis(); robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05); robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05); } - } // <-- both levels too low + } // <-- low level too low if (robot.LowRiserLeft.getPosition() <= 0.46 && robot.HighRiserLeft.getPosition() < 0.64) { if (System.currentTimeMillis() - lastStepTime >= 125) { lastStepTime = System.currentTimeMillis(); - robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05); + robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05 + topServoOffset); robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05); } } // <-- high level too low @@ -430,31 +431,31 @@ public class PhoenixTeleOPState extends CyberarmState { } if (OCD == 3) { // Medium junction - if (robot.LowRiserLeft.getPosition() > 0.46 && robot.HighRiserLeft.getPosition() > 0.8) { + if (robot.LowRiserLeft.getPosition() > 0.46) { if (System.currentTimeMillis() - lastStepTime >= 125) { lastStepTime = System.currentTimeMillis(); robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05); robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05); } - } // <-- both levels too high + } // <-- low level too high if (robot.LowRiserLeft.getPosition() <= 0.46 && robot.HighRiserLeft.getPosition() > 0.8) { if (System.currentTimeMillis() - lastStepTime >= 125) { lastStepTime = System.currentTimeMillis(); - robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05); + robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05 + topServoOffset); robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.05); } } // <-- top level too high - if (robot.LowRiserLeft.getPosition() < 0.44 && robot.HighRiserLeft.getPosition() < 0.79) { + if (robot.LowRiserLeft.getPosition() < 0.44) { if (System.currentTimeMillis() - lastStepTime >= 125) { lastStepTime = System.currentTimeMillis(); robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05); robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05); } - } // <-- both levels too low + } // <-- low level too low if (robot.LowRiserLeft.getPosition() > 0.44 && robot.LowRiserLeft.getPosition() <= 0.46 && robot.HighRiserLeft.getPosition() < 0.79) { if (System.currentTimeMillis() - lastStepTime >= 125) { lastStepTime = System.currentTimeMillis(); - robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05); + robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05 + topServoOffset); robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05); } } // <-- high level too low @@ -467,7 +468,7 @@ public class PhoenixTeleOPState extends CyberarmState { if (robot.HighRiserLeft.getPosition() < 0.84) { if (System.currentTimeMillis() - lastStepTime >= 125) { lastStepTime = System.currentTimeMillis(); - robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05); + robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05 + topServoOffset); robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05); } } From 9cb9806620feaab8be51760bd6409f88b7f59d28 Mon Sep 17 00:00:00 2001 From: Sodi Date: Sat, 31 Dec 2022 11:59:23 -0600 Subject: [PATCH 5/7] Compensating for servo offset --- .../TeleOp/states/PhoenixTeleOPState.java | 27 +++++++++---------- 1 file changed, 13 insertions(+), 14 deletions(-) diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java index 9195d8f..4927564 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java +++ b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java @@ -20,7 +20,7 @@ public class PhoenixTeleOPState extends CyberarmState { private int CyclingArmUpAndDown = 0; private double RobotRotation; private double RotationTarget, DeltaRotation; - private double MinimalPower = 0.25, topServoOffset = 0.05; + private double MinimalPower = 0.25, topServoOffset = -0.05; private GamepadChecker gamepad1Checker, gamepad2Checker; private int OCD; @@ -67,12 +67,11 @@ public class PhoenixTeleOPState extends CyberarmState { robot.LowRiserRight.setDirection(Servo.Direction.REVERSE); robot.LowRiserLeft.setPosition(0.45); robot.LowRiserRight.setPosition(0.45); - robot.HighRiserLeft.setPosition(0.45 + topServoOffset); - robot.HighRiserRight.setPosition(0.45); + robot.HighRiserLeft.setPosition(0.45); + robot.HighRiserRight.setPosition(0.45 + topServoOffset); robot.OdometerEncoder.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); robot.OdometerEncoderLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - topServoOffset = robot.HighRiserRight.getPosition() + 0.05; gamepad1Checker = new GamepadChecker(engine, engine.gamepad1); gamepad2Checker = new GamepadChecker(engine, engine.gamepad2); OCD = 0; @@ -306,7 +305,7 @@ public class PhoenixTeleOPState extends CyberarmState { if (System.currentTimeMillis() - lastStepTime >= 150) { lastStepTime = System.currentTimeMillis(); OCD = 0; - robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05 + topServoOffset); + robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05); robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05); } } @@ -317,7 +316,7 @@ public class PhoenixTeleOPState extends CyberarmState { if (System.currentTimeMillis() - lastStepTime >= 150) { lastStepTime = System.currentTimeMillis(); OCD = 0; - robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05 + topServoOffset); + robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05); robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.05); } } @@ -327,7 +326,7 @@ public class PhoenixTeleOPState extends CyberarmState { if (robot.HighRiserLeft.getPosition() < 0.85) { if (System.currentTimeMillis() - lastStepTime >= 150) { lastStepTime = System.currentTimeMillis(); - robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05 + topServoOffset); + robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05); robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05); } } @@ -345,7 +344,7 @@ public class PhoenixTeleOPState extends CyberarmState { if (robot.HighRiserLeft.getPosition() > 0.45 && robot.LowRiserLeft.getPosition() < 0.5) { if (System.currentTimeMillis() - lastStepTime >= 150) { lastStepTime = System.currentTimeMillis(); - robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05 + topServoOffset); + robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05); robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.05); } } @@ -388,7 +387,7 @@ public class PhoenixTeleOPState extends CyberarmState { } else if (robot.LowRiserLeft.getPosition() <= 0.46 && robot.HighRiserLeft.getPosition() > 0.46) { if (System.currentTimeMillis() - lastStepTime >= 125) { lastStepTime = System.currentTimeMillis(); - robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05 + topServoOffset); + robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05); robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.05); } } else if (robot.LowRiserLeft.getPosition() <= 0.46 && robot.HighRiserLeft.getPosition() <= 0.46) { @@ -407,7 +406,7 @@ public class PhoenixTeleOPState extends CyberarmState { if (robot.LowRiserLeft.getPosition() <= 0.46 && robot.LowRiserLeft.getPosition() > 0.44 && robot.HighRiserLeft.getPosition() > 0.66) { if (System.currentTimeMillis() - lastStepTime >= 125) { lastStepTime = System.currentTimeMillis(); - robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05 + topServoOffset); + robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05); robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.05); } } // <-- top level too high @@ -421,7 +420,7 @@ public class PhoenixTeleOPState extends CyberarmState { if (robot.LowRiserLeft.getPosition() <= 0.46 && robot.HighRiserLeft.getPosition() < 0.64) { if (System.currentTimeMillis() - lastStepTime >= 125) { lastStepTime = System.currentTimeMillis(); - robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05 + topServoOffset); + robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05); robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05); } } // <-- high level too low @@ -441,7 +440,7 @@ public class PhoenixTeleOPState extends CyberarmState { if (robot.LowRiserLeft.getPosition() <= 0.46 && robot.HighRiserLeft.getPosition() > 0.8) { if (System.currentTimeMillis() - lastStepTime >= 125) { lastStepTime = System.currentTimeMillis(); - robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05 + topServoOffset); + robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05); robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.05); } } // <-- top level too high @@ -455,7 +454,7 @@ public class PhoenixTeleOPState extends CyberarmState { if (robot.LowRiserLeft.getPosition() > 0.44 && robot.LowRiserLeft.getPosition() <= 0.46 && robot.HighRiserLeft.getPosition() < 0.79) { if (System.currentTimeMillis() - lastStepTime >= 125) { lastStepTime = System.currentTimeMillis(); - robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05 + topServoOffset); + robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05); robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05); } } // <-- high level too low @@ -468,7 +467,7 @@ public class PhoenixTeleOPState extends CyberarmState { if (robot.HighRiserLeft.getPosition() < 0.84) { if (System.currentTimeMillis() - lastStepTime >= 125) { lastStepTime = System.currentTimeMillis(); - robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05 + topServoOffset); + robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05); robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05); } } From c0ace898af497b5058d8f00923fb8edbc205a4d0 Mon Sep 17 00:00:00 2001 From: Cyberarm Date: Sat, 31 Dec 2022 12:56:21 -0600 Subject: [PATCH 6/7] Tested and refactored blackboard --- .../cyberarm/engine/V2/CyberarmEngine.java | 41 +++++++--- .../Engines/LeftSideAutonomousEngine.java | 2 +- .../RightFourConeAutonomousEngine.java | 2 +- .../Engines/RightSideAutonomousEngine.java | 2 +- .../Autonomous/States/ConeIdentification.java | 14 ++-- .../States/DriverParkPlaceState.java | 2 +- .../Autonomous/States/PathDecision.java | 2 +- .../cyberarm/engines/TasksEngine.java | 76 +++++++++++++++++++ 8 files changed, 119 insertions(+), 22 deletions(-) create mode 100644 TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/TasksEngine.java diff --git a/TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmEngine.java b/TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmEngine.java index e78ba8d..8fc7885 100644 --- a/TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmEngine.java +++ b/TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmEngine.java @@ -10,7 +10,6 @@ import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Act import java.lang.reflect.Constructor; import java.lang.reflect.InvocationTargetException; -import java.util.HashMap; import java.util.concurrent.ConcurrentHashMap; import java.util.concurrent.CopyOnWriteArrayList; @@ -32,7 +31,7 @@ public abstract class CyberarmEngine extends OpMode { // Array to Hold Tasks final private CopyOnWriteArrayList backgroundTasks = new CopyOnWriteArrayList<>(); // HashMap to store data for States and Tasks - private ConcurrentHashMap blackboard = new ConcurrentHashMap<>(); + final private ConcurrentHashMap blackboard = new ConcurrentHashMap<>(); private int activeStateIndex = 0; // Index of currently running state private boolean isRunning; // Whether engine is running or not @@ -139,7 +138,7 @@ public abstract class CyberarmEngine extends OpMode { // Background tasks for (CyberarmState task : backgroundTasks) { - initState(task); + stateTelemetry(task); } } @@ -293,21 +292,44 @@ public abstract class CyberarmEngine extends OpMode { /** * Retrieve value from blackboard * @param key String to use to look up value - * @return Returns Object which should be autocast to the correct type + * @return Returns T of stored Object */ - @SuppressWarnings("unchecked") - public T blackboard_get(String key) { + public T blackboardGet(String key) { return (T) blackboard.get(key); } + public String blackboardGetString(String key) { + return (String) blackboard.get(key); + } + + public int blackboardGetInt(String key) { + return (int) blackboard.get(key); + } + + public long blackboardGetLong(String key) { + return (long) blackboard.get(key); + } + + public float blackboardGetFloat(String key) { + return (float) blackboard.get(key); + } + + public double blackboardGetDouble(String key) { + return (double) blackboard.get(key); + } + + public boolean blackboardGetBoolean(String key) { + return (boolean) blackboard.get(key); + } + /** * Set value of key to value * @param key String * @param value Object * @return Returns T */ - @SuppressWarnings("unchecked") - public T blackboard_set(String key, Object value) { + + public T blackboardSet(String key, T value) { blackboard.put(key, value); return (T) value; @@ -319,8 +341,7 @@ public abstract class CyberarmEngine extends OpMode { * @param value Object * @return Returns T */ - @SuppressWarnings("unchecked") - public T blackboard_remove(String key, Object value) { + public T blackboardRemove(String key, T value) { blackboard.remove(key); return (T) value; diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftSideAutonomousEngine.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftSideAutonomousEngine.java index 5f69767..38559f1 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftSideAutonomousEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftSideAutonomousEngine.java @@ -96,6 +96,6 @@ public class LeftSideAutonomousEngine extends CyberarmEngine { public void loop() { super.loop(); - telemetry.addData("BlackBoard Input", blackboard_get("parkPlace")); + telemetry.addData("BlackBoard Input", blackboardGetString("parkPlace")); } } diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightFourConeAutonomousEngine.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightFourConeAutonomousEngine.java index 0032c62..f2a8dc2 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightFourConeAutonomousEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightFourConeAutonomousEngine.java @@ -148,6 +148,6 @@ public class RightFourConeAutonomousEngine extends CyberarmEngine { public void loop() { super.loop(); - telemetry.addData("BlackBoard Input", blackboard_get("parkPlace")); + telemetry.addData("BlackBoard Input", blackboardGetString("parkPlace")); } } diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightSideAutonomousEngine.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightSideAutonomousEngine.java index c714223..fef79d6 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightSideAutonomousEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightSideAutonomousEngine.java @@ -97,6 +97,6 @@ public class RightSideAutonomousEngine extends CyberarmEngine { public void loop() { super.loop(); - telemetry.addData("BlackBoard Input", blackboard_get("parkPlace")); + telemetry.addData("BlackBoard Input", blackboardGetString("parkPlace")); } } diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/ConeIdentification.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/ConeIdentification.java index 6bb512a..43c6473 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/ConeIdentification.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/ConeIdentification.java @@ -21,7 +21,7 @@ public class ConeIdentification extends CyberarmState { @Override public void init() { - engine.blackboard_set("parkPlace", "1"); + engine.blackboardSet("parkPlace", "1"); robot.tfod.activate(); initTime = System.currentTimeMillis(); } @@ -50,11 +50,11 @@ public class ConeIdentification extends CyberarmState { engine.telemetry.addData("- Size (Width/Height)","%.0f / %.0f", width, height); if (recognition.getLabel().equals("#2")) { - engine.telemetry.addData("#2", engine.blackboard_set("parkPlace", "2")); + engine.telemetry.addData("#2", engine.blackboardSet("parkPlace", "2")); } else if (recognition.getLabel().equals("#3")) { - engine.telemetry.addData("#3",engine.blackboard_set("parkPlace", "3")); + engine.telemetry.addData("#3",engine.blackboardSet("parkPlace", "3")); } else { - engine.telemetry.addData("#1", engine.blackboard_set("parkPlace", "1")); + engine.telemetry.addData("#1", engine.blackboardSet("parkPlace", "1")); } } } @@ -90,12 +90,12 @@ public class ConeIdentification extends CyberarmState { bestConfidence = recognition.getConfidence(); if (recognition.getLabel().equals("2 Bulb")) { - engine.blackboard_set("parkPlace", "2"); + engine.blackboardSet("parkPlace", "2"); } else if (recognition.getLabel().equals("3 Panel")) { - engine.blackboard_set("parkPlace", "3"); + engine.blackboardSet("parkPlace", "3"); } else { - engine.blackboard_set("parkPlace", "1"); + engine.blackboardSet("parkPlace", "1"); } } } diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverParkPlaceState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverParkPlaceState.java index f1dd8cf..382615f 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverParkPlaceState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverParkPlaceState.java @@ -43,7 +43,7 @@ public class DriverParkPlaceState extends CyberarmState { setHasFinished(true); return; } - String placement = engine.blackboard_get("parkPlace"); + String placement = engine.blackboardGetString("parkPlace"); if (placement != null) { if (!placement.equals(intendedPlacement)){ setHasFinished(true); diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/PathDecision.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/PathDecision.java index 483468d..87923bf 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/PathDecision.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/PathDecision.java @@ -16,7 +16,7 @@ public class PathDecision extends CyberarmState { @Override public void exec() { - String placement = engine.blackboard_get("parkPlace"); + String placement = engine.blackboardGetString("parkPlace"); setHasFinished(true); } diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/TasksEngine.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/TasksEngine.java new file mode 100644 index 0000000..eea9c75 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/TasksEngine.java @@ -0,0 +1,76 @@ +package org.timecrafters.minibots.cyberarm.engines; + +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.cyberarm.engine.V2.CyberarmEngine; +import org.cyberarm.engine.V2.CyberarmState; + +@TeleOp(name = "TasksEngine", group = "testing") +public class TasksEngine extends CyberarmEngine { + @Override + public void setup() { + addTask(new CyberarmState() { + @Override + public void init() { + engine.blackboardSet("counter", 0); + engine.blackboardSet("string", "I IS STRING"); + engine.blackboardSet("boolean", true); + } + + @Override + public void exec() { + } + + @Override + public void telemetry() { + engine.telemetry.addData("TASK 1", engine.blackboardGetInt("counter")); + engine.telemetry.addData("TASK 1", "I am a task!"); + } + }); + + addTask(new CyberarmState() { + int lastCount = 0, blackboardLastCount = 0; + int count = 0; + double lastRuntime = 0; + @Override + public void exec() { + if (runTime() - lastRuntime >= 1000.0) { + lastRuntime = runTime(); + lastCount = count; + blackboardLastCount = engine.blackboardGetInt("counter"); + } + engine.blackboardSet("counter", engine.blackboardGetInt("counter") + 1); + count++; + } + + @Override + public void telemetry() { + engine.telemetry.addData("TASK 2", engine.blackboardGetString("string")); + engine.telemetry.addData("TASK 2", engine.blackboardGetBoolean("boolean")); + engine.telemetry.addData("TASK 2", engine.blackboardGetInt("counter") - blackboardLastCount); + engine.telemetry.addData("TASK 2", count - lastCount); + engine.telemetry.addData("TASK 2", engine.blackboardGetString("string_NULL") == null); + } + }); + + addState(new CyberarmState() { + @Override + public void exec() { + if (runTime() >= 10_000) { + setHasFinished(true); + } + } + }); + + addParallelStateToLastState(new CyberarmState() { + @Override + public void exec() { + } + + @Override + public void telemetry() { + engine.telemetry.addData("Parallel state 1", "Hello There"); + } + }); + } +} From b598b0f97f2bf2bc8e2e4a784adc70621f765768 Mon Sep 17 00:00:00 2001 From: Sodi Date: Tue, 3 Jan 2023 20:36:17 -0600 Subject: [PATCH 7/7] Fixed the buggy B-button run-to, added variables instead of constants --- .../TeleOp/states/PhoenixTeleOPState.java | 89 +++++++++++-------- 1 file changed, 52 insertions(+), 37 deletions(-) diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java index 4927564..76fe85e 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java +++ b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java @@ -21,6 +21,14 @@ public class PhoenixTeleOPState extends CyberarmState { private double RobotRotation; private double RotationTarget, DeltaRotation; private double MinimalPower = 0.25, topServoOffset = -0.05; + private double servoCollectLow = 0.45; //Low servos, A button + private double servoCollectHigh = 0.55; //High servos, A button + private double servoLowLow = 0.45; //Low servos, X button + private double servoLowHigh = 0.75; //High servos, X button + private double servoMedLow = 0.45; //Low servos, B button + private double servoMedHigh = 0.9; //High servos, B button + private double servoHighLow = 0.8; //Low servos, Y button + private double servoHighHigh = 0.9; //High servos, Y button private GamepadChecker gamepad1Checker, gamepad2Checker; private int OCD; @@ -301,7 +309,7 @@ public class PhoenixTeleOPState extends CyberarmState { } if (engine.gamepad2.right_bumper) { - if (robot.HighRiserLeft.getPosition() < 1.0) { + if (robot.HighRiserLeft.getPosition() < 1.0 - Math.abs(topServoOffset)) { if (System.currentTimeMillis() - lastStepTime >= 150) { lastStepTime = System.currentTimeMillis(); OCD = 0; @@ -377,108 +385,115 @@ public class PhoenixTeleOPState extends CyberarmState { OCD = 4; } - if (OCD == 1) { //Ground junction - if (robot.LowRiserLeft.getPosition() >= 0.46) { - if (System.currentTimeMillis() - lastStepTime >= 125) { + if (OCD == 1) { //Ground junction/Collect + if (robot.LowRiserLeft.getPosition() >= servoCollectLow + 0.01) { + if (System.currentTimeMillis() - lastStepTime >= 120) { lastStepTime = System.currentTimeMillis(); robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05); robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05); } - } else if (robot.LowRiserLeft.getPosition() <= 0.46 && robot.HighRiserLeft.getPosition() > 0.46) { - if (System.currentTimeMillis() - lastStepTime >= 125) { + } else if (robot.LowRiserLeft.getPosition() <= servoCollectLow && + robot.HighRiserLeft.getPosition() > servoCollectHigh) { + if (System.currentTimeMillis() - lastStepTime >= 120) { lastStepTime = System.currentTimeMillis(); robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05); robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.05); } - } else if (robot.LowRiserLeft.getPosition() <= 0.46 && robot.HighRiserLeft.getPosition() <= 0.46) { + } else if (robot.LowRiserLeft.getPosition() <= servoCollectLow + 0.01 && + robot.HighRiserLeft.getPosition() <= servoCollectHigh) { OCD = 0; } } if (OCD == 2) { //low junction - if (robot.LowRiserLeft.getPosition() > 0.46) { - if (System.currentTimeMillis() - lastStepTime >= 125) { + if (robot.LowRiserLeft.getPosition() > servoLowLow + 0.01) { + if (System.currentTimeMillis() - lastStepTime >= 120) { lastStepTime = System.currentTimeMillis(); robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05); robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05); } } // <-- low level too high - if (robot.LowRiserLeft.getPosition() <= 0.46 && robot.LowRiserLeft.getPosition() > 0.44 && robot.HighRiserLeft.getPosition() > 0.66) { - if (System.currentTimeMillis() - lastStepTime >= 125) { + if (robot.LowRiserLeft.getPosition() <= servoLowLow + 0.01 && + robot.LowRiserLeft.getPosition() > servoLowLow - 0.01 && + robot.HighRiserLeft.getPosition() > servoLowHigh) { + if (System.currentTimeMillis() - lastStepTime >= 120) { lastStepTime = System.currentTimeMillis(); robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05); robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.05); } } // <-- top level too high - if (robot.LowRiserLeft.getPosition() < 0.44) { - if (System.currentTimeMillis() - lastStepTime >= 125) { + if (robot.LowRiserLeft.getPosition() < servoLowLow - 0.01) { + if (System.currentTimeMillis() - lastStepTime >= 120) { lastStepTime = System.currentTimeMillis(); robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05); robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05); } } // <-- low level too low - if (robot.LowRiserLeft.getPosition() <= 0.46 && robot.HighRiserLeft.getPosition() < 0.64) { - if (System.currentTimeMillis() - lastStepTime >= 125) { + if (robot.LowRiserLeft.getPosition() <= servoLowLow + 0.01 && + robot.HighRiserLeft.getPosition() < servoLowHigh - 0.01) { + if (System.currentTimeMillis() - lastStepTime >= 120) { lastStepTime = System.currentTimeMillis(); robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05); robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05); } } // <-- high level too low - if (robot.LowRiserLeft.getPosition() > 0.44 && robot.LowRiserLeft.getPosition() <= 0.46 && robot.HighRiserLeft.getPosition() > 0.64 && robot.HighRiserLeft.getPosition() <= 0.66) { + if (robot.LowRiserLeft.getPosition() > servoLowLow - 0.01 && + robot.LowRiserLeft.getPosition() <= servoLowLow + 0.01 && + robot.HighRiserLeft.getPosition() > servoLowHigh - 0.01 && + robot.HighRiserLeft.getPosition() <= servoLowHigh + 0.01) { OCD = 0; } } if (OCD == 3) { // Medium junction - if (robot.LowRiserLeft.getPosition() > 0.46) { - if (System.currentTimeMillis() - lastStepTime >= 125) { + if (robot.LowRiserLeft.getPosition() > servoMedLow + 0.01) { + if (System.currentTimeMillis() - lastStepTime >= 120) { lastStepTime = System.currentTimeMillis(); robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05); robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05); } } // <-- low level too high - if (robot.LowRiserLeft.getPosition() <= 0.46 && robot.HighRiserLeft.getPosition() > 0.8) { - if (System.currentTimeMillis() - lastStepTime >= 125) { + if (robot.LowRiserLeft.getPosition() <= servoMedLow + 0.01 && + robot.HighRiserLeft.getPosition() > servoMedHigh + 0.01)/* <-- high level too high*/ { + if (System.currentTimeMillis() - lastStepTime >= 120) { lastStepTime = System.currentTimeMillis(); robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05); robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.05); } - } // <-- top level too high - if (robot.LowRiserLeft.getPosition() < 0.44) { - if (System.currentTimeMillis() - lastStepTime >= 125) { - lastStepTime = System.currentTimeMillis(); - robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05); - robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05); - } - } // <-- low level too low - if (robot.LowRiserLeft.getPosition() > 0.44 && robot.LowRiserLeft.getPosition() <= 0.46 && robot.HighRiserLeft.getPosition() < 0.79) { - if (System.currentTimeMillis() - lastStepTime >= 125) { + } + if (robot.LowRiserLeft.getPosition() < servoMedLow + 0.01 && + robot.HighRiserLeft.getPosition() < servoMedHigh - 0.01)/* <-- high level too low*/ { + if (System.currentTimeMillis() - lastStepTime >= 120) { lastStepTime = System.currentTimeMillis(); robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05); robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05); } - } // <-- high level too low - if (robot.LowRiserLeft.getPosition() > 0.44 && robot.LowRiserLeft.getPosition() <= 0.6 && robot.HighRiserLeft.getPosition() > 0.79 && robot.HighRiserLeft.getPosition() <= 0.81) { + } + if (robot.LowRiserLeft.getPosition() > servoMedLow - 0.01 && + robot.LowRiserLeft.getPosition() <= servoMedLow && + robot.HighRiserLeft.getPosition() > servoMedHigh - 0.01 && + robot.HighRiserLeft.getPosition() <= servoMedHigh) { OCD = 0; } } if (OCD == 4) { // High Junction - if (robot.HighRiserLeft.getPosition() < 0.84) { - if (System.currentTimeMillis() - lastStepTime >= 125) { + if (robot.HighRiserLeft.getPosition() < servoHighHigh - 0.01) { + if (System.currentTimeMillis() - lastStepTime >= 120) { lastStepTime = System.currentTimeMillis(); robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05); robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05); } } - if (robot.LowRiserLeft.getPosition() <= 0.64) { - if (System.currentTimeMillis() - lastStepTime >= 125) { + if (robot.LowRiserLeft.getPosition() < servoHighLow - 0.01) { + if (System.currentTimeMillis() - lastStepTime >= 120) { lastStepTime = System.currentTimeMillis(); robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05); robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05); } } - if (robot.HighRiserLeft.getPosition() >= 0.84 && robot.LowRiserLeft.getPosition() >= 0.64) { + if (robot.HighRiserLeft.getPosition() >= servoHighHigh && + robot.LowRiserLeft.getPosition() >= servoHighLow) { OCD = 0; } }