From 7d3649cb1da9d9894b357f2bbacded219031815b Mon Sep 17 00:00:00 2001 From: Spencer Date: Tue, 7 Dec 2021 20:37:10 -0600 Subject: [PATCH] more auto work started configuring camera --- .../Engines/BlueWarehouseEngine.java | 58 +++++++++++++ .../Engines/RedWarehouseEngine.java | 18 ++-- .../Autonomous/States/TensorFlowState.java | 86 +++++++++++++++++++ .../Autonomous/States/TurretArmRiser.java | 8 +- .../Autonomous/States/VuforiaState.java | 36 -------- .../Competition/Common/Robot.java | 4 +- .../Autonomous/TensorFlowCheck.java | 2 +- 7 files changed, 163 insertions(+), 49 deletions(-) create mode 100644 TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/Engines/BlueWarehouseEngine.java create mode 100644 TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TensorFlowState.java delete mode 100644 TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/VuforiaState.java diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/Engines/BlueWarehouseEngine.java b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/Engines/BlueWarehouseEngine.java new file mode 100644 index 0000000..7a1603e --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/Engines/BlueWarehouseEngine.java @@ -0,0 +1,58 @@ +package org.timecrafters.FreightFrenzy.Competition.Autonomous.Engines; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.cyberarm.engine.V2.CyberarmEngine; +import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.CollectorToggle; +import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.DriveState; +import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.TensorFlowState; +import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.TurretArmExtension; +import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.TurretArmRiser; +import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.TurretOrbit; +import org.timecrafters.FreightFrenzy.Competition.Common.Robot; + +@Autonomous (name = "Blue WareHouse", group = "blue") +public class BlueWarehouseEngine extends CyberarmEngine { + Robot robot; + + @Override + public void setup() { + this.robot = new Robot(this); + + addState(new TurretOrbit(robot, robot.turretServoWhite, robot.whiteMag, "BlueWarehouseAutonomous", "01_0")); + addState(new TensorFlowState(robot, robot.whiteArmRiser, robot.whiteArmBobbin, "BlueWarehouseAutonomous", "01_1")); +// addState(new TurretArmExtension(robot, robot.whiteArmBobbin, "BlueWarehouseAutonomous", "02_0")); +// addState(new TurretArmRiser(robot, robot.whiteArmRiser, "BlueWarehouseAutonomous", "03_0_middle")); +// addState(new TurretArmExtension(robot, robot.whiteArmBobbin, "BlueWarehouseAutonomous", "04_0_middle")); + addState(new CollectorToggle(robot, robot.collectorWhite, "BlueWarehouseAutonomous", "05_0")); + addState(new CollectorToggle(robot, robot.collectorWhite, "BlueWarehouseAutonomous", "06_0")); + addState(new TurretArmExtension(robot, robot.whiteArmBobbin, "BlueWarehouseAutonomous", "07_0")); + addState(new DriveState(robot,"BlueWarehouseAutonomous", "08_0")); + addState(new DriveState(robot, "BlueWarehouseAutonomous", "09_0")); + addState(new DriveState(robot, "BlueWarehouseAutonomous", "10_0")); + + } + + public void loop() { + super.loop(); + + telemetry.addData("white arm extension", robot.whiteArmBobbin.getCurrentPosition()); + telemetry.addData("White Riser Arm", robot.whiteArmRiser.getCurrentPosition()); + telemetry.addData("white Turret Switch", robot.whiteMag.isPressed()); + telemetry.addData("white Turret orbit Power", robot.turretServoWhite.getPower()); + telemetry.addData("White Door Position", robot.whiteDispenser.getPosition()); + telemetry.addLine(); + + telemetry.addData("Orange Riser Arm", robot.orangeArmRiser.getCurrentPosition()); + telemetry.addData("orange arm extension", robot.orangeArmBobbin.getCurrentPosition()); + telemetry.addData("orange Turret Switch", robot.orangeMag.isPressed()); + telemetry.addData("orange Turret Orbit Power", robot.turretServoOrange.getPower()); + telemetry.addData("Orange Door Position", robot.orangeDispenser.getPosition()); + telemetry.addLine(); + + telemetry.addData("driveWarehouseLeft", robot.driveWarehouseLeft.getCurrentPosition()); + telemetry.addData("driveWarehouseRight", robot.driveWarehouseRight.getCurrentPosition()); + telemetry.addData("driveGoalLeft", robot.driveGoalLeft.getCurrentPosition()); + telemetry.addData("driveGoalRight", robot.driveGoalRight.getCurrentPosition()); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/Engines/RedWarehouseEngine.java b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/Engines/RedWarehouseEngine.java index ddb2c62..81cbfc9 100644 --- a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/Engines/RedWarehouseEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/Engines/RedWarehouseEngine.java @@ -6,7 +6,9 @@ import org.cyberarm.engine.V2.CyberarmEngine; import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.CollectorToggle; import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.DepositorDoor; import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.DriveState; +import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.TensorFlowState; import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.TurretArmExtension; +import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.TurretArmRiser; import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.TurretOrbit; import org.timecrafters.FreightFrenzy.Competition.Common.Robot; @Autonomous (name = "Red WareHouse", group = "red") @@ -18,13 +20,17 @@ public class RedWarehouseEngine extends CyberarmEngine { this.robot = new Robot(this); addState(new TurretOrbit(robot, robot.turretServoWhite, robot.whiteMag, "RedWarehouseAutonomous", "01_0")); - addState(new TurretArmExtension(robot, robot.whiteArmBobbin, "RedWarehouseAutonomous", "02_0")); - addState(new CollectorToggle(robot, robot.collectorWhite, "RedWarehouseAutonomous", "03_0")); - addState(new CollectorToggle(robot, robot.collectorWhite, "RedWarehouseAutonomous", "04_0")); - addState(new TurretArmExtension(robot, robot.whiteArmBobbin, "RedWarehouseAutonomous", "05_0")); - addState(new DriveState(robot,"RedWarehouseAutonomous", "07_0")); - addState(new DriveState(robot, "RedWarehouseAutonomous", "08_0")); + addState(new TensorFlowState(robot, robot.whiteArmRiser, robot.whiteArmBobbin, "RedWarehouseAutonomous", "01_1")); +// addState(new TurretArmExtension(robot, robot.whiteArmBobbin, "RedWarehouseAutonomous", "02_0")); +// addState(new TurretArmRiser(robot, robot.whiteArmRiser, "RedWarehouseAutonomous", "03_0_middle")); +// addState(new TurretArmExtension(robot, robot.whiteArmBobbin, "RedWarehouseAutonomous", "04_0")); + addState(new CollectorToggle(robot, robot.collectorWhite, "RedWarehouseAutonomous", "05_0")); + addState(new CollectorToggle(robot, robot.collectorWhite, "RedWarehouseAutonomous", "06_0")); + addState(new TurretArmExtension(robot, robot.whiteArmBobbin, "RedWarehouseAutonomous", "07_0")); + addState(new DriveState(robot,"RedWarehouseAutonomous", "08_0")); addState(new DriveState(robot, "RedWarehouseAutonomous", "09_0")); + addState(new TurretOrbit(robot, robot.turretServoWhite, null, "RedWarehouseAutonomous", "10_0")); + addState(new DriveState(robot, "RedWarehouseAutonomous", "10_1")); } diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TensorFlowState.java b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TensorFlowState.java new file mode 100644 index 0000000..eb1c3ff --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TensorFlowState.java @@ -0,0 +1,86 @@ +package org.timecrafters.FreightFrenzy.Competition.Autonomous.States; + +import com.qualcomm.robotcore.hardware.DcMotor; + +import org.cyberarm.engine.V2.CyberarmState; +import org.firstinspires.ftc.robotcore.external.tfod.Recognition; +import org.timecrafters.FreightFrenzy.Competition.Common.Robot; + +import java.util.List; + +public class TensorFlowState extends CyberarmState { + + Robot robot; + private List recognitions; + private double checkTime; + private int manualPath; + private int path = 0; + + private String groupName; + private DcMotor armRiser, armExtension; + + public TensorFlowState(Robot robot, DcMotor armRiser, DcMotor armExtension, String groupName, String actionName) { + this.robot = robot; + this.groupName = groupName; + this.armRiser = armRiser; + this.armExtension = armExtension; + } + + @Override + public void start() { + robot.activateVuforia(); + robot.activateTensorflow(); + } + + @Override + public void exec() { + recognitions = robot.tensorflowDetections(); +// + if (runTime() < checkTime) { + if (manualPath != -1) { + + if (recognitions != null) { + if (recognitions.size() == 1) { + Recognition recognition = recognitions.get(0); + + if (recognition.getLabel().equals("duck")){ + if (recognition.getLeft() < 320) { + path = 0; + } else { + path = 1; + } + } + } else { + path = 2; + } + } + } + } else { + if (path == 0){ + addState(new TurretArmExtension(robot, armExtension, groupName, "02_0")); + addState(new TurretArmRiser(robot, armRiser, groupName, "03_0_bottom")); + addState(new TurretArmExtension(robot, armExtension, groupName, "04_0_bottom")); + } else if (path == 1){ + addState(new TurretArmExtension(robot, armExtension, groupName, "02_0")); + addState(new TurretArmRiser(robot, armRiser, groupName, "03_0_middle")); + addState(new TurretArmExtension(robot, armExtension, groupName, "04_0_middle")); + } else { + addState(new TurretArmExtension(robot, armExtension, groupName, "02_0")); + addState(new TurretArmRiser(robot, armRiser, groupName, "03_0_top")); + addState(new TurretArmExtension(robot, armExtension, groupName, "04_0_top")); + } + + setHasFinished(true); + } + } + + @Override + public void telemetry() { + for (Recognition recognition : robot.tensorflowDetections()) { + engine.telemetry.addData("Label", recognition.getLabel()); + engine.telemetry.addData("Left", recognition.getLeft()); + engine.telemetry.addData("Top", recognition.getTop()); + engine.telemetry.addData("Confidence", recognition.getConfidence()); + } + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TurretArmRiser.java b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TurretArmRiser.java index e0cd681..f397eb1 100644 --- a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TurretArmRiser.java +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TurretArmRiser.java @@ -12,12 +12,12 @@ public class TurretArmRiser extends CyberarmState { private int targetPosition, tolerance; private double power; - public TurretArmRiser(Robot robot, DcMotor motor, int targetPosition, double power, int tolerance) { + public TurretArmRiser(Robot robot, DcMotor motor, String groupName, String actionName) { this.robot = robot; this.motor = motor; - this.targetPosition = targetPosition; - this.power = power; - this.tolerance = tolerance; + this.targetPosition = robot.configuration.variable(groupName, actionName, "targetPosition").value(); + this.power = robot.configuration.variable(groupName, actionName, "power").value(); + this.tolerance = robot.configuration.variable(groupName, actionName, "tolerance").value(); } @Override diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/VuforiaState.java b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/VuforiaState.java deleted file mode 100644 index 50f2115..0000000 --- a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/VuforiaState.java +++ /dev/null @@ -1,36 +0,0 @@ -package org.timecrafters.FreightFrenzy.Competition.Autonomous.States; - -import org.cyberarm.engine.V2.CyberarmState; -import org.firstinspires.ftc.robotcore.external.tfod.Recognition; -import org.timecrafters.FreightFrenzy.Competition.Common.Robot; - -public class VuforiaState extends CyberarmState { - - Robot robot; - - public VuforiaState(Robot robot) { - this.robot = robot; - } - - @Override - public void start() { - robot.activateVuforia(); - robot.activateTensorflow(); - } - - @Override - public void exec() { - for (Recognition recognition : robot.tensorflowDetections()) { - } - } - - @Override - public void telemetry() { - for (Recognition recognition : robot.tensorflowDetections()) { - engine.telemetry.addData("Label", recognition.getLabel()); - engine.telemetry.addData("Left", recognition.getLeft()); - engine.telemetry.addData("Top", recognition.getTop()); - engine.telemetry.addData("Confidence", recognition.getConfidence()); - } - } -} diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Common/Robot.java b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Common/Robot.java index 3c260be..15166d5 100644 --- a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Common/Robot.java +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Common/Robot.java @@ -101,8 +101,8 @@ public class Robot { initCollector(); initDepositor(); initCarousel(); -// initVuforia(); -// initTensorflow(); + initVuforia(); + initTensorflow(); } private void initMagnetSwitches() { diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/TensorFlowCheck.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/TensorFlowCheck.java index fe382a4..62a49a0 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/TensorFlowCheck.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/TensorFlowCheck.java @@ -85,8 +85,8 @@ public class TensorFlowCheck extends CyberarmState { setHasFinished(true); } - } + } @Override public void telemetry() { engine.telemetry.addData("Chosen Path", path);