From 1856a94a0df02936e01e1870c8560533af622466 Mon Sep 17 00:00:00 2001 From: SpencerPiha <88354753+SpencerPiha@users.noreply.github.com> Date: Fri, 21 Oct 2022 14:54:36 -0500 Subject: [PATCH] Camera Implemented, and I have Created all of the states in order of how the autonomous will go. just need to implement values --- .../Engines/TestAutonomousEngine.java | 37 +++++++++++++- .../testing/states/PrototypeBot1.java | 50 +++++++++++++++++-- 2 files changed, 82 insertions(+), 5 deletions(-) diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/TestAutonomousEngine.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/TestAutonomousEngine.java index 5e8f67e..83a3dec 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/TestAutonomousEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/TestAutonomousEngine.java @@ -3,7 +3,10 @@ package org.timecrafters.Autonomous.Engines; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import org.cyberarm.engine.V2.CyberarmEngine; +import org.timecrafters.Autonomous.States.CollectorState; +import org.timecrafters.Autonomous.States.DriverState; import org.timecrafters.Autonomous.States.LowerArm; +import org.timecrafters.Autonomous.States.RotationState; import org.timecrafters.Autonomous.States.UpperArm; import org.timecrafters.testing.states.PrototypeBot1; @@ -16,8 +19,38 @@ public class TestAutonomousEngine extends CyberarmEngine { public void setup() { robot = new PrototypeBot1(this); - addState(new UpperArm(robot, "TestAutonomous", "01-0")); - addState(new LowerArm(robot, "TestAutonomous", "02-0")); + //drive to high pole + addState(new DriverState(robot, "TestAutonomous", "01-0")); + //turn towards high pole + addState(new RotationState(robot, "TestAutonomous", "02-0")); + //lift the upper arm + addState(new UpperArm(robot, "TestAutonomous", "03-0")); + //lift the lower arm + addState(new LowerArm(robot, "TestAutonomous", "04-0")); + //drive forward + addState(new DriverState(robot, "TestAutonomous", "05-0")); + //lower the bottom arm to get closer + addState(new LowerArm(robot, "TestAutonomous", "06-0")); + //make collector release the cone + addState(new CollectorState(robot, "TestAutnomous", "07-0")); + //lift the lower arm to clear the pole + addState(new LowerArm(robot, "TestAutonomous", "08-0")); + //Drive Backwards + addState(new DriverState(robot, "TestAutonomous", "09-0")); + // Rotate to either set up for cones to grab or park somewhere + addState(new RotationState(robot, "TestAutonomous", "10-0")); + // lower the bottom arm so we dont fall over + addState(new LowerArm(robot, "TestAutonomous", "11-0")); + // lower the upper arm so we dont fall over + addState(new UpperArm(robot, "TestAutonomous", "12-0")); + //either dont move if your in the right zone otherwise drive forward a specific variable if we aren't already in the right spot + addState(new DriverState(robot, "TestAutonomous", "13-0")); + + + + + + } diff --git a/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java b/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java index bbeb323..faac5ea 100644 --- a/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java +++ b/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java @@ -7,12 +7,31 @@ import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.Servo; 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.Position; import org.firstinspires.ftc.robotcore.external.navigation.Velocity; +import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer; +import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector; import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration; public class PrototypeBot1 { + private static final String TFOD_MODEL_ASSET = "PowerPlay.tflite"; + + private static final String[] LABELS = { + "1 Bolt", + "2 Bulb", + "3 Panel" + }; + + private static final String VUFORIA_KEY = + "Abmu1jv/////AAABmYzrcgDEi014nv+wD6PkEPVnOlV2pI3S9sGUMMR/X7hF72x20rP1JcVtsU0nI6VK0yUlYbCSA2k+yMo4hQmPDBvrqeqAgXKa57ilPhW5e1cB3BEevP+9VoJ9QYFhKA3JJTiuFS50WQeuFy3dp0gOPoqHL3XClRFZWbhzihyNnLXgXlKiq+i5GbfONECucQU2DgiuuxYlCaeNdUHl1X5C2pO80zZ6y7PYAp3p0ciXJxqfBoVAklhd69avaAE5Z84ctKscvcbxCS16lq81X7XgIFjshLoD/vpWa300llDG83+Y777q7b5v7gsUCZ6FiuK152Rd272HLuBRhoTXAt0ug9Baq5cz3sn0sAIEzSHX1nah"; + + private VuforiaLocalizer vuforia; + + private TFObjectDetector tfod; + public Servo LowRiserLeft, LowRiserRight, HighRiserLeft, HighRiserRight; private final CyberarmEngine engine; @@ -29,6 +48,8 @@ public class PrototypeBot1 { public PrototypeBot1(CyberarmEngine engine) { this.engine = engine; + initVuforia(); + initTfod(); setupRobot(); } @@ -40,7 +61,7 @@ public class PrototypeBot1 { parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC; parameters.loggingEnabled = false; - this.imu = engine.hardwareMap.get(BNO055IMU.class,"imu"); + this.imu = engine.hardwareMap.get(BNO055IMU.class, "imu"); imu.initialize(parameters); imu.startAccelerationIntegration(new Position(), new Velocity(), 10); @@ -78,6 +99,29 @@ public class PrototypeBot1 { backRightDrive.setDirection(DcMotorSimple.Direction.FORWARD); backRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - } - } + + private void initVuforia(){ + /* + * Configure Vuforia by creating a Parameter object, and passing it to the Vuforia engine. + */ + VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(); + + parameters.vuforiaLicenseKey = VUFORIA_KEY; + parameters.cameraName = engine.hardwareMap.get(WebcamName.class, "Webcam 1"); + + // Instantiate the Vuforia engine + vuforia = ClassFactory.getInstance().createVuforia(parameters); + } + + private void 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; + tfod = ClassFactory.getInstance().createTFObjectDetector(tfodParameters, vuforia); + tfod.loadModelFromAsset(TFOD_MODEL_ASSET, LABELS); + } + }