diff --git a/TeamCode/src/main/java/dev/cyberarm/engine/V2/CyberarmEngine.java b/TeamCode/src/main/java/dev/cyberarm/engine/V2/CyberarmEngine.java index a515e48..184b993 100644 --- a/TeamCode/src/main/java/dev/cyberarm/engine/V2/CyberarmEngine.java +++ b/TeamCode/src/main/java/dev/cyberarm/engine/V2/CyberarmEngine.java @@ -416,6 +416,21 @@ public abstract class CyberarmEngine extends OpMode { * @param groupName Group name */ public void setupFromConfig(TimeCraftersConfiguration configuration, String packageName, Object object, Class objectClass, String groupName) { + setupFromConfig(configuration, packageName, object, objectClass, groupName, null); + } + + /** + * Automatically populates states from a TimeCraftersConfiguration Group actions + * requires action comments to start with an @ character followed by the class name + * state must have a construction that takes 3 arguments: object, groupName, and actionName + * @param configuration TimeCraftersConfiguration + * @param packageName Package name where states are defined + * @param object Object to pass as first argument to states constructor + * @param objectClass Class to cast object to + * @param groupName Group name + * @param insertAfterState state to insert states AFTER + */ + public void setupFromConfig(TimeCraftersConfiguration configuration, String packageName, Object object, Class objectClass, String groupName, CyberarmState insertAfterState) { CyberarmState lastState = null; String lastActionName = null; String[] lastActionNameSplit = new String[0]; @@ -446,7 +461,11 @@ public abstract class CyberarmEngine extends OpMode { { lastState.addParallelState(state); } else { - addState(state); + if (insertAfterState != null) { + insertAfterState.addState(state); + } else { + addState(state); + } lastState = state; } diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java index 0718295..1cc2334 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java @@ -8,8 +8,12 @@ import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.IMU; import com.qualcomm.robotcore.hardware.Servo; +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.tfod.TFObjectDetector; +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.tfod.TfodProcessor; import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration; import java.sql.Time; @@ -68,11 +72,29 @@ public class RedCrabMinibot { public final TimeCraftersConfiguration config; - public RedCrabMinibot() { + public enum Path { + LEFT, + CENTER, + RIGHT + } + + public final TfodProcessor tfod; + public final VisionPortal visionPortal; + + public RedCrabMinibot(boolean autonomous) { engine = CyberarmEngine.instance; config = new TimeCraftersConfiguration("cyberarm_RedCrab"); + if (autonomous) { + tfod = TfodProcessor.easyCreateWithDefaults(); + visionPortal = VisionPortal.easyCreateWithDefaults( + engine.hardwareMap.get(WebcamName.class, "Webcam 1"), tfod); + } else { + tfod = null; + visionPortal = null; + } + /// IMU /// /// ------------------------------------------------------------------------------------ /// imu = engine.hardwareMap.get(IMU.class, "imu"); // | Ctrl Hub, I2C Port: 0 diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueAudienceEngine.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueAudienceEngine.java index 78f857f..0262035 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueAudienceEngine.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueAudienceEngine.java @@ -14,7 +14,7 @@ public class RedCrabAutonomousBlueAudienceEngine extends CyberarmEngine { setupFromConfig( new TimeCraftersConfiguration("cyberarm_RedCrab"), "dev.cyberarm.minibots.red_crab.states", - new RedCrabMinibot(), + new RedCrabMinibot(true), RedCrabMinibot.class, "Autonomous_BLUE_Audience" ); diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueBackstageEngine.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueBackstageEngine.java index e4b080c..e14b4cf 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueBackstageEngine.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueBackstageEngine.java @@ -14,7 +14,7 @@ public class RedCrabAutonomousBlueBackstageEngine extends CyberarmEngine { setupFromConfig( new TimeCraftersConfiguration("cyberarm_RedCrab"), "dev.cyberarm.minibots.red_crab.states", - new RedCrabMinibot(), + new RedCrabMinibot(true), RedCrabMinibot.class, "Autonomous_BLUE_Backstage" ); diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedAudienceEngine.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedAudienceEngine.java index 46271b6..b15f54c 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedAudienceEngine.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedAudienceEngine.java @@ -14,7 +14,7 @@ public class RedCrabAutonomousRedAudienceEngine extends CyberarmEngine { setupFromConfig( new TimeCraftersConfiguration("cyberarm_RedCrab"), "dev.cyberarm.minibots.red_crab.states", - new RedCrabMinibot(), + new RedCrabMinibot(true), RedCrabMinibot.class, "Autonomous_RED_Audience" ); diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedBackstageEngine.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedBackstageEngine.java index a285070..e1ab5e0 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedBackstageEngine.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedBackstageEngine.java @@ -14,7 +14,7 @@ public class RedCrabAutonomousRedBackstageEngine extends CyberarmEngine { setupFromConfig( new TimeCraftersConfiguration("cyberarm_RedCrab"), "dev.cyberarm.minibots.red_crab.states", - new RedCrabMinibot(), + new RedCrabMinibot(true), RedCrabMinibot.class, "Autonomous_RED_Backstage" ); diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabTeleOpEngine.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabTeleOpEngine.java index fadb4d8..45d138e 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabTeleOpEngine.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabTeleOpEngine.java @@ -10,6 +10,6 @@ import dev.cyberarm.minibots.red_crab.states.Pilot; public class RedCrabTeleOpEngine extends CyberarmEngine { @Override public void setup() { - addState(new Pilot(new RedCrabMinibot())); + addState(new Pilot(new RedCrabMinibot(false))); } } diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/PathEnactor.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/PathEnactor.java new file mode 100644 index 0000000..528cead --- /dev/null +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/PathEnactor.java @@ -0,0 +1,38 @@ +package dev.cyberarm.minibots.red_crab.states; + +import dev.cyberarm.engine.V2.CyberarmState; +import dev.cyberarm.minibots.red_crab.RedCrabMinibot; + +public class PathEnactor extends CyberarmState { + private final RedCrabMinibot robot; + private String pathGroupName; + public PathEnactor(RedCrabMinibot robot, String groupName, String actionName) { + this.robot = robot; + } + + @Override + public void start() { + String path; + switch (engine.blackboardGetInt("AutonomousPath")) { + case 1: + path = "CENTER"; + break; + case 2: + path = "RIGHT"; + break; + default: + path = "LEFT"; + break; + } + + this.pathGroupName = String.format("AutonomousPixelPath_%s", path); + } + + @Override + public void exec() { + engine.setupFromConfig( + robot.config, "dev.cyberarm.minibots.red_crab.states", robot, robot.getClass(), pathGroupName, this); + + finished(); + } +} diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/PathSelector.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/PathSelector.java new file mode 100644 index 0000000..8780c82 --- /dev/null +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/PathSelector.java @@ -0,0 +1,74 @@ +package dev.cyberarm.minibots.red_crab.states; + +import org.firstinspires.ftc.robotcore.external.tfod.Recognition; + +import java.util.ArrayList; +import java.util.List; + +import dev.cyberarm.engine.V2.CyberarmState; +import dev.cyberarm.minibots.red_crab.RedCrabMinibot; + +public class PathSelector extends CyberarmState { + private final RedCrabMinibot robot; + private final int timeoutMS; + private final int fallbackPath; + private List recognitions = new ArrayList<>(); + + public PathSelector(RedCrabMinibot robot, String groupName, String actionName) { + this.robot = robot; + + this.timeoutMS = robot.config.variable(groupName, actionName, "timeoutMS").value(); + this.fallbackPath = robot.config.variable(groupName, actionName, "fallbackPath").value(); + } + + @Override + public void init() { + robot.tfod.setClippingMargins(0, 0, 0, 0); + robot.tfod.setMinResultConfidence(0.8f); + + engine.blackboardSet("autonomousPath", fallbackPath); + } + + @Override + public void exec() { + recognitions = robot.tfod.getRecognitions(); + for (Recognition recognition : recognitions) { + double x = (recognition.getLeft() + recognition.getRight()) / 2; + double y = (recognition.getTop() + recognition.getBottom()) / 2; + + if (recognition.getLabel().equals("pixel")) { + if (x < 120) { + engine.blackboardSet("autonomousPath", RedCrabMinibot.Path.LEFT.ordinal()); + } else if (x >= 120 && x < 240) { + engine.blackboardSet("autonomousPath", RedCrabMinibot.Path.CENTER.ordinal()); + } else { + engine.blackboardSet("autonomousPath", RedCrabMinibot.Path.RIGHT.ordinal()); + } + } + } + + if (runTime() >= timeoutMS) { + robot.visionPortal.close(); + robot.tfod.shutdown(); + finished(); + } + } + + @Override + public void telemetry() { + engine.telemetry.addLine(); + + engine.telemetry.addData("# Objects Detected", recognitions.size()); + + for(Recognition recognition : recognitions) { + double x = (recognition.getLeft() + recognition.getRight()) / 2; + double y = (recognition.getTop() + recognition.getBottom()) / 2; + + engine.telemetry.addLine(); + + engine.telemetry.addData("Image", "%s (%.0f %% Conf.)", recognition.getLabel(), recognition.getConfidence() * 100); + engine.telemetry.addData("- Position", "%.0f / %.0f", x, y); + engine.telemetry.addData("- Size", "%.0f x %.0f", recognition.getWidth(), recognition.getHeight()); + } + } +}