From 1b50fd015b907b2aba18f847cf648c2b8d9382e5 Mon Sep 17 00:00:00 2001 From: cyberarm Date: Thu, 26 Jan 2023 10:16:31 -0600 Subject: [PATCH] Initial implementation of camera, changed access of CyberarmEngine#setupFromConfig to public from protected, refactored autonomous engines to only use the 2 logical sides instead of the 4 for states, a hardware fault during autonomous will cause the opmode to abort, misc. changes. --- .../cyberarm/engine/V2/CyberarmEngine.java | 2 +- .../minibots/cyberarm/chiron/Robot.java | 55 ++++++++++-- .../chiron/engines/AutonomousEngine.java | 9 ++ .../AutonomousBlueLeftSideEngine.java | 2 +- .../AutonomousBlueRightSideEngine.java | 2 +- .../AutonomousRedLeftSideEngine.java | 2 +- .../AutonomousRedRightSideEngine.java | 2 +- .../autonomous/SelectParkingPosition.java | 34 +++++++- .../states/autonomous/SignalProcessor.java | 87 ++++++++++++++++++- .../teleop/DrivetrainDriverControl.java | 6 ++ 10 files changed, 187 insertions(+), 14 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 8fc7885..334e0d8 100644 --- a/TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmEngine.java +++ b/TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmEngine.java @@ -415,7 +415,7 @@ public abstract class CyberarmEngine extends OpMode { * @param objectClass Class to cast object to * @param groupName Group name */ - protected void setupFromConfig(TimeCraftersConfiguration configuration, String packageName, Object object, Class objectClass, String groupName) { + public void setupFromConfig(TimeCraftersConfiguration configuration, String packageName, Object object, Class objectClass, String groupName) { CyberarmState lastState = null; String lastActionName = null; String[] lastActionNameSplit = new String[0]; 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 48b26dd..1556c64 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 @@ -17,9 +17,13 @@ import com.qualcomm.robotcore.hardware.Servo; import com.qualcomm.robotcore.hardware.ServoImplEx; import org.cyberarm.engine.V2.CyberarmEngine; +import org.firstinspires.ftc.robotcore.external.ClassFactory; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer; +import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector; import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration; import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action; import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Variable; @@ -43,6 +47,7 @@ public class Robot { public boolean wristManuallyControlled = false; public boolean automaticAntiTipActive = false; public boolean hardwareFault = false; + public String hardwareFaultMessage = ""; private Status status = Status.OKAY, lastStatus = Status.OKAY; private final CopyOnWriteArrayList reportedStatuses = new CopyOnWriteArrayList<>(); @@ -82,6 +87,11 @@ public class Robot { private WristPosition wristTargetPosition, wristCurrentPosition; private double wristPositionChangeTime, wristPositionChangeRequestTime; + private static final String VUFORIA_KEY = + "Abmu1jv/////AAABmYzrcgDEi014nv+wD6PkEPVnOlV2pI3S9sGUMMR/X7hF72x20rP1JcVtsU0nI6VK0yUlYbCSA2k+yMo4hQmPDBvrqeqAgXKa57ilPhW5e1cB3BEevP+9VoJ9QYFhKA3JJTiuFS50WQeuFy3dp0gOPoqHL3XClRFZWbhzihyNnLXgXlKiq+i5GbfONECucQU2DgiuuxYlCaeNdUHl1X5C2pO80zZ6y7PYAp3p0ciXJxqfBoVAklhd69avaAE5Z84ctKscvcbxCS16lq81X7XgIFjshLoD/vpWa300llDG83+Y777q7b5v7gsUCZ6FiuK152Rd272HLuBRhoTXAt0ug9Baq5cz3sn0sAIEzSHX1nah"; + private final VuforiaLocalizer vuforia; + private final TFObjectDetector tfod; + private boolean LEDStatusToggle = false; private double lastLEDStatusAnimationTime = 0; @@ -198,11 +208,38 @@ public class Robot { expansionHub.setPattern(ledPatternOkay()); } + // Webcam + vuforia = initVuforia(); + tfod = initTfod(); + // INITIALIZE AFTER EVERYTHING ELSE to prevent use before set crashes this.fieldLocalizer.setRobot(this); this.fieldLocalizer.standardSetup(); } + private VuforiaLocalizer initVuforia() { + VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(); + + parameters.vuforiaLicenseKey = VUFORIA_KEY; + parameters.cameraName = engine.hardwareMap.get(WebcamName.class, "Webcam 1"); + + return ClassFactory.getInstance().createVuforia(parameters); + } + + private TFObjectDetector initTfod() { + int tfodMonitorViewId = engine.hardwareMap.appContext.getResources().getIdentifier( + "tfodMonitorViewId", "id", engine.hardwareMap.appContext.getPackageName()); + TFObjectDetector.Parameters tfodParameters = new TFObjectDetector.Parameters(tfodMonitorViewId); + tfodParameters.minResultConfidence = 0.75f; + tfodParameters.isModelTensorFlow2 = true; + tfodParameters.inputSize = 300; + TFObjectDetector tfod = ClassFactory.getInstance().createTFObjectDetector(tfodParameters, vuforia); + + tfod.loadModelFromAsset("PowerPlay.tflite", "1 Bolt", "2 Bulb", "3 Panel"); + + return tfod; + } + public void standardTelemetry() { engine.telemetry.addLine(); @@ -210,6 +247,7 @@ public class Robot { engine.telemetry.addLine("DATA"); engine.telemetry.addData(" Robot Status", status); engine.telemetry.addData(" Hardware Fault", hardwareFault); + engine.telemetry.addData(" Hardware Fault Message", hardwareFaultMessage); engine.telemetry.addLine(); // Motor Powers @@ -392,8 +430,11 @@ public class Robot { wrist.setPower(0); } - if (getVoltage() < 9.75) { + double voltage = getVoltage(); + if (voltage < 9.75) { reportStatus(Status.DANGER); + hardwareFaultMessage = "Battery voltage to low! (" + voltage + " volts)"; + hardwareFault = true; } @@ -528,6 +569,14 @@ public class Robot { public double getDiameter() { return diameter; } + public double getVoltage() { + return engine.hardwareMap.voltageSensor.iterator().next().getVoltage(); + } + + public TFObjectDetector getTfod() { return tfod; } + + public VuforiaLocalizer getVuforia() { return vuforia; } + public TimeCraftersConfiguration getConfiguration() { return configuration; } public FieldLocalizer getFieldLocalizer() { return fieldLocalizer; } @@ -667,10 +716,6 @@ public class Robot { arm.setPower(0.75); } - public double getVoltage() { - return engine.hardwareMap.voltageSensor.iterator().next().getVoltage(); - } - public double facing() { double imuDegrees = -imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES); diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/AutonomousEngine.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/AutonomousEngine.java index aa09dbd..230b459 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/AutonomousEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/AutonomousEngine.java @@ -1,11 +1,14 @@ package org.timecrafters.minibots.cyberarm.chiron.engines; +import android.util.Log; + import org.cyberarm.engine.V2.CyberarmEngine; import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration; import org.timecrafters.minibots.cyberarm.chiron.Robot; import org.timecrafters.minibots.cyberarm.chiron.tasks.FieldLocalizer; public class AutonomousEngine extends CyberarmEngine { + private static final String TAG = "CHIRON|AutonomousEngine"; protected Robot robot; protected FieldLocalizer fieldLocalizer; protected TimeCraftersConfiguration configuration; @@ -49,6 +52,12 @@ public class AutonomousEngine extends CyberarmEngine { super.loop(); robot.standardTelemetry(); + + if (robot.hardwareFault) { + Log.e(TAG, "Hardware fault detected! Aborting run. Message: " + robot.hardwareFaultMessage); + + stop(); + } } @Override diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousBlueLeftSideEngine.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousBlueLeftSideEngine.java index 9d4e684..1ee4228 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousBlueLeftSideEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousBlueLeftSideEngine.java @@ -8,7 +8,7 @@ import org.timecrafters.minibots.cyberarm.chiron.engines.AutonomousEngine; public class AutonomousBlueLeftSideEngine extends AutonomousEngine { @Override public void setup() { - actionsGroupName = "AutonomousBlueLeftSide"; + actionsGroupName = "AutonomousLeftSide"; tuningGroupName = "Autonomous"; tuningActionName = "Tuning_Blue_LeftSide"; diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousBlueRightSideEngine.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousBlueRightSideEngine.java index 0f55457..08ad0b0 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousBlueRightSideEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousBlueRightSideEngine.java @@ -8,7 +8,7 @@ import org.timecrafters.minibots.cyberarm.chiron.engines.AutonomousEngine; public class AutonomousBlueRightSideEngine extends AutonomousEngine { @Override public void setup() { - actionsGroupName = "AutonomousBlueRightSide"; + actionsGroupName = "AutonomousRightSide"; tuningGroupName = "Autonomous"; tuningActionName = "Tuning_Blue_RightSide"; diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousRedLeftSideEngine.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousRedLeftSideEngine.java index 847a85c..db4e3d0 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousRedLeftSideEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousRedLeftSideEngine.java @@ -8,7 +8,7 @@ import org.timecrafters.minibots.cyberarm.chiron.engines.AutonomousEngine; public class AutonomousRedLeftSideEngine extends AutonomousEngine { @Override public void setup() { - actionsGroupName = "AutonomousRedLeftSide"; + actionsGroupName = "AutonomousLeftSide"; tuningGroupName = "Autonomous"; tuningActionName = "Tuning_Red_LeftSide"; diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousRedRightSideEngine.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousRedRightSideEngine.java index 7da0668..e8d4272 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousRedRightSideEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousRedRightSideEngine.java @@ -8,7 +8,7 @@ import org.timecrafters.minibots.cyberarm.chiron.engines.AutonomousEngine; public class AutonomousRedRightSideEngine extends AutonomousEngine { @Override public void setup() { - actionsGroupName = "AutonomousRedRightSide"; + actionsGroupName = "AutonomousRightSide"; tuningGroupName = "Autonomous"; tuningActionName = "Tuning_Red_RightSide"; diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/SelectParkingPosition.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/SelectParkingPosition.java index 9edac4a..bb33df3 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/SelectParkingPosition.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/SelectParkingPosition.java @@ -7,15 +7,45 @@ public class SelectParkingPosition extends CyberarmState { private final Robot robot; private final String groupName, actionName; + private final double timeInMS; + private final boolean stateDisabled; + public SelectParkingPosition(Robot robot, String groupName, String actionName) { this.robot = robot; this.groupName = groupName; this.actionName = actionName; + + timeInMS = robot.getConfiguration().variable(groupName, actionName, "timeInMS").value(); + + stateDisabled = !robot.getConfiguration().action(groupName, actionName).enabled; + } + + @Override + public void start() { + int pos = engine.blackboardGetInt("parking_position"); + + engine.setupFromConfig( + robot.getConfiguration(), + "org.timecrafters.minibots.cyberarm.chiron.states.autonomous", + robot, + Robot.class, + "" + groupName + "_Parking_" + pos); } @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/SignalProcessor.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/SignalProcessor.java index 5965d8f..b2cdde7 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 @@ -1,26 +1,109 @@ package org.timecrafters.minibots.cyberarm.chiron.states.autonomous; import org.cyberarm.engine.V2.CyberarmState; +import org.firstinspires.ftc.robotcore.external.tfod.Recognition; import org.timecrafters.minibots.cyberarm.chiron.Robot; +import java.util.List; + public class SignalProcessor extends CyberarmState { private final Robot robot; private final String groupName, actionName; + private final double timeInMS, minConfidence; + private final int fallbackPosition; + private final boolean stateDisabled; + public SignalProcessor(Robot robot, String groupName, String actionName) { this.robot = robot; this.groupName = groupName; this.actionName = actionName; + + timeInMS = robot.getConfiguration().variable(groupName, actionName, "timeInMS").value(); + minConfidence = robot.getConfiguration().variable(groupName, actionName, "minConfidence").value(); + fallbackPosition = robot.getConfiguration().variable(groupName, actionName, "fallbackPosition").value(); + + stateDisabled = !robot.getConfiguration().action(groupName, actionName).enabled; } @Override public void start() { - engine.blackboardSet("parking_position", System.currentTimeMillis() % 3); + engine.blackboardSet("parking_position", fallbackPosition); + + robot.getTfod().activate(); } @Override public void exec() { - // FIXME: NO OP + if (stateDisabled) { + stop(); + + return; + } + + if (runTime() >= timeInMS) { + stop(); + + return; + } + + if (robot.getTfod() != null) { + // getUpdatedRecognitions() will return null if no new information is available since + // the last time that call was made. + List updatedRecognitions = robot.getTfod().getUpdatedRecognitions(); + + if (updatedRecognitions != null) { + for (Recognition recognition : updatedRecognitions) { + switch (recognition.getLabel()) { + case "1 Bolt": + engine.blackboardSet("parking_position", 1); + + break; + case "2 Bulb": + engine.blackboardSet("parking_position", 2); + + break; + case "3 Panel": + engine.blackboardSet("parking_position", 3); + + break; + } + } + } + } + } + + @Override + public void telemetry() { + if (robot.getTfod() != null) { + // getUpdatedRecognitions() will return null if no new information is available since + // the last time that call was made. + List updatedRecognitions = robot.getTfod().getUpdatedRecognitions(); + + if (updatedRecognitions != null) { + engine.telemetry.addData("# Objects Detected", updatedRecognitions.size()); + + // step through the list of recognitions and display image position/size information for each one + // Note: "Image number" refers to the randomized image orientation/number + for (Recognition recognition : updatedRecognitions) { + double col = (recognition.getLeft() + recognition.getRight()) / 2 ; + double row = (recognition.getTop() + recognition.getBottom()) / 2 ; + double width = Math.abs(recognition.getRight() - recognition.getLeft()) ; + double height = Math.abs(recognition.getTop() - recognition.getBottom()) ; + + engine.telemetry.addData(""," "); + engine.telemetry.addData("Image", "%s (%.0f %% Conf.)", recognition.getLabel(), recognition.getConfidence() * 100 ); + engine.telemetry.addData("- Position (Row/Col)","%.0f / %.0f", row, col); + engine.telemetry.addData("- Size (Width/Height)","%.0f / %.0f", width, height); + } + } + } + } + + @Override + public void stop() { setHasFinished(true); + + robot.getTfod().deactivate(); } } diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/DrivetrainDriverControl.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/DrivetrainDriverControl.java index 881643a..02bfa42 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/DrivetrainDriverControl.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/DrivetrainDriverControl.java @@ -128,6 +128,12 @@ public class DrivetrainDriverControl extends CyberarmState { // DEBUG: Toggle hardware fault if (button.equals("guide")) { robot.hardwareFault = !robot.hardwareFault; + + if (robot.hardwareFault) { + robot.hardwareFaultMessage = "Manually triggered."; + } else { + robot.hardwareFaultMessage = ""; + } } if (button.equals("back")) {