From a8e3634fafd0f2d86143b0e3d133f5a5ac524449 Mon Sep 17 00:00:00 2001 From: sihan Date: Mon, 4 Mar 2024 10:25:54 -0600 Subject: [PATCH] ?? --- .../ftc/teamcode/hardware/Robot.java | 2 + .../ftc/teamcode/opmodes/AutoBase.java | 47 ++++ .../opmodes/AutoRedFarTwoPlusTwo.java | 125 ++++++--- .../ftc/teamcode/opmodes/MainTeleOp.java | 28 +- .../opmodes/RedFarTwoPlusTwoTest.java | 254 ++++++++++++++++++ 5 files changed, 402 insertions(+), 54 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBase.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/RedFarTwoPlusTwoTest.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java index 42403f5..73e4e98 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java @@ -1,5 +1,6 @@ package org.firstinspires.ftc.teamcode.hardware; +import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.gamepad1; import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.gamepad2; import static org.firstinspires.ftc.teamcode.util.Configurables.ARMACCSCORE; import static org.firstinspires.ftc.teamcode.util.Configurables.ARMPICKUPSTACK; @@ -349,6 +350,7 @@ public class Robot { case NEUTRAL: if (runtime > delay) { this.getArm().armRest(); + gamepad1.rumble(300); pickupMacroState = pickupMacroStates.IDLE; } break; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBase.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBase.java new file mode 100644 index 0000000..e912c3a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBase.java @@ -0,0 +1,47 @@ +package org.firstinspires.ftc.teamcode.opmodes; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.hardware.Robot; +import org.firstinspires.ftc.teamcode.opmodes.AutoRedFarTwoPlusTwo.autoState; + +@Config +public abstract class AutoBase extends LinearOpMode { + protected Pose2d initialPosition; + Robot robot; + String randomization; + String parkLocation; + double runtime; + + autoState state = autoState.PURPLE; + int delay; + + public abstract void followTrajectories(); + + @Override + public void runOpMode() { + this.telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + this.robot = new Robot().init(hardwareMap); + this.robot.getCamera().initTargetingCamera(); + this.initialPosition = new Pose2d(-34, -59.5, Math.toRadians(270)); + this.robot.getDrive().setPoseEstimate(initialPosition); + + while (!this.isStarted()) { + parkLocation = "RIGHT"; + this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPosition()); + randomization = String.valueOf(this.robot.getCamera().getStartingPosition()); + this.telemetry.addData("Park Position", parkLocation); + this.telemetry.addData("Delay", delay); + this.telemetry.update(); + + } + while (state != autoState.STOP) { + followTrajectories(); + robot.getDrive().update(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFarTwoPlusTwo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFarTwoPlusTwo.java index 72d165a..47830e3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFarTwoPlusTwo.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFarTwoPlusTwo.java @@ -1,26 +1,16 @@ package org.firstinspires.ftc.teamcode.opmodes; -import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.roadrunner.geometry.Pose2d; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import org.firstinspires.ftc.teamcode.hardware.Robot; import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants; import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive; import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder; @Config -@Autonomous(name = "AutoRedFar2+2") -public class AutoRedFarTwoPlusTwo extends LinearOpMode { - protected Pose2d initialPosition; - private Robot robot; - private String randomization; - private String parkLocation; - private int delay = 10000; - +@Autonomous(name = "AutoRedFar2+2",preselectTeleOp = "Main TeleOp") +public class AutoRedFarTwoPlusTwo extends AutoBase { //Pose2ds //Preloads final static Pose2d RIGHT_PRELOAD_ONE = new Pose2d(-40, -33.5, Math.toRadians(230)); @@ -46,6 +36,15 @@ public class AutoRedFarTwoPlusTwo extends LinearOpMode { final static Pose2d PARKLEFT = new Pose2d(45, -12, Math.toRadians(0)); final static Pose2d PARKLEFT2 = new Pose2d(60, -12, Math.toRadians(0)); + protected enum autoState{ + STOP, + PURPLE, + YELLOW, + FLIPYELLOW, + SCOREYELLOW, + PARK, + } + protected void scorePreloadOne() { TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); switch (randomization) { @@ -206,7 +205,6 @@ public class AutoRedFarTwoPlusTwo extends LinearOpMode { } else if (gamepad2.dpad_right) { parkLocation = "RIGHT"; }} - protected void delaySet(){ if (gamepad2.dpad_up && delay<13000) { delay+=1000; @@ -216,37 +214,82 @@ public class AutoRedFarTwoPlusTwo extends LinearOpMode { sleep(100); } } - @Override - public void runOpMode() throws InterruptedException { - this.telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); - this.robot = new Robot().init(hardwareMap); - this.robot.getCamera().initTargetingCamera(); - this.initialPosition = new Pose2d(-34, -59.5, Math.toRadians(270)); - this.robot.getDrive().setPoseEstimate(initialPosition); - // Do super fancy chinese shit - while (!this.isStarted()) { - parkLocation = "RIGHT"; - this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPosition()); - randomization = String.valueOf(this.robot.getCamera().getStartingPosition()); - this.telemetry.addData("Park Position", parkLocation); - this.telemetry.addData("Delay", delay); - this.telemetry.update(); + + public void followTrajectories(){ + TrajectorySequenceBuilder builder; + switch (state){ + case PURPLE: + builder = this.robot.getTrajectorySequenceBuilder(); + switch (randomization) { + case "LEFT": + builder.lineToLinearHeading(LEFT_PRELOAD); + break; + case "CENTER": + builder.lineToLinearHeading(CENTER_PRELOAD); + break; + case "RIGHT": + builder.lineToLinearHeading(RIGHT_PRELOAD_ONE); + builder.lineToLinearHeading(RIGHT_PRELOAD_TWO); + break; + } + this.robot.getDrive().followTrajectorySequenceAsync(builder.build()); + + if (!robot.getDrive().isBusy()){ + state = autoState.YELLOW; + } + break; + case YELLOW: + builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(LEAVE); + builder.lineToLinearHeading(READY_TRUSS); + builder.lineToLinearHeading(TO_BOARD); + switch (randomization) { + case "LEFT": + builder.splineToConstantHeading(SCORE_BOARD_LEFT.vec(),Math.toRadians(0)); + break; + case "CENTER": + builder.splineToConstantHeading(SCORE_BOARD_MID.vec(),Math.toRadians(0)); + break; + case "RIGHT": + builder.splineToConstantHeading(SCORE_BOARD_RIGHT.vec(),Math.toRadians(0)); + break; + } + this.robot.getDrive().followTrajectorySequenceAsync(builder.build()); + runtime = getRuntime(); + state = autoState.FLIPYELLOW; + break; + case FLIPYELLOW: + if (getRuntime() > runtime + 5) { + this.robot.getArm().armSecondaryScore(); + this.robot.getWrist().wristScore(); + state = autoState.SCOREYELLOW; + } + break; + case SCOREYELLOW: + if (!robot.getDrive().isBusy()){ + this.robot.getClaw().open(); + state = autoState.PARK; + } + break; + case PARK: + } - scorePreloadOne(); - goBackToWhereYouCameFrom(); -// scoreBoard(); - score(); - this.robot.getClaw().open(); - backTruss(); - this.robot.getClaw().close(); - sleep(200); - goBackstage(); - scoreBoardStack(); - scoreTest(); - park(); } -} + +// scorePreloadOne(); +// goBackToWhereYouCameFrom(); +//// scoreBoard(); +// score(); +// this.robot.getClaw().open(); +// backTruss(); +// this.robot.getClaw().close(); +// sleep(200); +// goBackstage(); +// scoreBoardStack(); +// scoreTest(); +// park(); + } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/MainTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/MainTeleOp.java index f3382ef..9dbb458 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/MainTeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/MainTeleOp.java @@ -20,12 +20,12 @@ public class MainTeleOp extends OpMode { GamepadEx controller2 = new GamepadEx(gamepad2); public void loop() { - boolean slideUp = gamepad2.dpad_up; - boolean plane = gamepad2.right_bumper; - boolean slideDown = gamepad2.dpad_left; + boolean slideUp = controller2.isDown(GamepadKeys.Button.DPAD_UP); + boolean slideDown = controller2.isDown(GamepadKeys.Button.DPAD_LEFT); boolean hang = gamepad2.left_bumper; - boolean hangRelease = gamepad2.y; - boolean hangPlane = gamepad2.dpad_right; + boolean hangRelease = gamepad2.right_bumper; + boolean hangPlane = gamepad2.y; + boolean plane = gamepad2.dpad_right; //Drive robot.getDrive().setInput(gamepad1, gamepad2); //slides @@ -33,26 +33,28 @@ public class MainTeleOp extends OpMode { this.robot.getSlides().slideUp(); } else if (slideDown) { this.robot.getSlides().slideDown(); - } else { + } else if (controller2.wasJustReleased(GamepadKeys.Button.DPAD_DOWN) + || controller2.wasJustReleased(GamepadKeys.Button.DPAD_LEFT)) { this.robot.getSlides().slideStop(); } - //Macros - this.robot.getLed().LED(); - this.robot.pickupMacro(controller2,getRuntime()); + this.robot.pickupMacro(controller2, getRuntime()); //DPADDOWN //Arm and Wrist - if (controller2.wasJustPressed(GamepadKeys.Button.X)){ + if (controller2.wasJustPressed(GamepadKeys.Button.X)) { this.robot.getArm().armSecondaryScore(); this.robot.getWrist().wristScore(); - } else if (controller2.wasJustPressed(GamepadKeys.Button.A)){ + } else if (controller2.wasJustPressed(GamepadKeys.Button.A)) { this.robot.getArm().armRest(); this.robot.getWrist().wristPickup(); - this.robot.getClaw().close(); } //Claw - if (controller2.wasJustPressed(GamepadKeys.Button.B)){ + if (controller2.wasJustPressed(GamepadKeys.Button.B)) { + gamepad1.rumble(300); + } else if (controller2.isDown(GamepadKeys.Button.B)){ this.robot.getClaw().open(); + } else if (controller2.wasJustReleased(GamepadKeys.Button.B)){ + this.robot.getClaw().close(); } //Hang if (hang) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/RedFarTwoPlusTwoTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/RedFarTwoPlusTwoTest.java new file mode 100644 index 0000000..98a8689 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/RedFarTwoPlusTwoTest.java @@ -0,0 +1,254 @@ +package org.firstinspires.ftc.teamcode.opmodes; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.hardware.Robot; +import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants; +import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive; +import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder; + +@Config +@Autonomous(name = "TestRedFar2+2") +public class RedFarTwoPlusTwoTest extends LinearOpMode { + protected Pose2d initialPosition; + private Robot robot; + private String randomization; + private String parkLocation; + private int delay = 10000; + + //Pose2ds + //Preloads + final static Pose2d RIGHT_PRELOAD_ONE = new Pose2d(-40, -33.5, Math.toRadians(230)); + final static Pose2d RIGHT_PRELOAD_TWO = new Pose2d(-29.5, -31, Math.toRadians(200)); + final static Pose2d RIGHT_PRELOAD_GETOUT = new Pose2d(-43, -42, Math.toRadians(0)); + final static Pose2d CENTER_PRELOAD = new Pose2d(-35, -28, Math.toRadians(270)); + final static Pose2d LEFT_PRELOAD = new Pose2d(-45, -27, Math.toRadians(270)); + //Ready Truss + final static Pose2d LEAVE = new Pose2d(-43, -40, Math.toRadians(270)); + final static Pose2d READY_TRUSS = new Pose2d(-43, -56.5, Math.toRadians(0)); + final static Pose2d TO_BOARD = new Pose2d(26, -56.5, Math.toRadians(0)); + final static Pose2d READY_SCORE = new Pose2d(37, -34, Math.toRadians(0)); + final static Pose2d SCORE_BOARD_LEFT = new Pose2d(55, -27, Math.toRadians(5)); + final static Pose2d SCORE_BOARD_MID = new Pose2d(55.5, -32.2, Math.toRadians(5)); + final static Pose2d SCORE_BOARD_RIGHT = new Pose2d(55, -39, Math.toRadians(5)); + final static Pose2d GET_STACK = new Pose2d(-47, -30, Math.toRadians(0)); + final static Pose2d PICKUP_STACK = new Pose2d(-61.5, -30.5, Math.toRadians(0)); + final static Pose2d PICKUP_STACK_MID = new Pose2d(-61.5, -32.5, Math.toRadians(0)); + final static Pose2d READY_SCORESTACK = new Pose2d(50, -41, Math.toRadians(0)); + final static Pose2d PICKUP_STACK_LEFT = new Pose2d(-60.5, -33, Math.toRadians(0)); + final static Pose2d PARK = new Pose2d(45, -60, Math.toRadians(0)); + final static Pose2d PARK2 = new Pose2d(60, -63, Math.toRadians(0)); + final static Pose2d PARKLEFT = new Pose2d(45, -12, Math.toRadians(0)); + final static Pose2d PARKLEFT2 = new Pose2d(60, -12, Math.toRadians(0)); + + protected void scorePreloadOne() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + switch (randomization) { + case "LEFT": + builder.lineToLinearHeading(LEFT_PRELOAD); + break; + case "CENTER": + builder.lineToLinearHeading(CENTER_PRELOAD); + break; + case "RIGHT": + builder.lineToLinearHeading(RIGHT_PRELOAD_ONE); + builder.lineToLinearHeading(RIGHT_PRELOAD_TWO); + break; + } + this.robot.getDrive().followTrajectorySequence(builder.build()); + } + + protected void goBackToWhereYouCameFrom() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(LEAVE, + MecanumDrive.getVelocityConstraint(60, Math.toRadians(60), DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(90)); + builder.lineToLinearHeading(READY_TRUSS, + MecanumDrive.getVelocityConstraint(60, Math.toRadians(60), DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(90)); + builder.lineToLinearHeading(TO_BOARD, + MecanumDrive.getVelocityConstraint(90, Math.toRadians(90), DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(90)); + this.robot.getDrive().followTrajectorySequence(builder.build()); + } + + protected void scoreBoard(){ + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(READY_SCORE); + builder.addTemporalMarker(.2, robot.getArm()::armSecondaryScore); + builder.addTemporalMarker(.2, robot.getSlides()::slideAutoStacks); + builder.addTemporalMarker(.2, robot.getWrist()::wristScore); + this.robot.getDrive().followTrajectorySequence(builder.build()); + } + + protected void score(){ + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + + switch (randomization) { + case "LEFT": + builder.lineToLinearHeading(SCORE_BOARD_LEFT, + MecanumDrive.getVelocityConstraint(40, 40, DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(40)); + break; + case "CENTER": + builder.lineToLinearHeading(SCORE_BOARD_MID, + MecanumDrive.getVelocityConstraint(40, 40, DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(20)); + break; + case "RIGHT": + builder.lineToLinearHeading(SCORE_BOARD_RIGHT, + MecanumDrive.getVelocityConstraint(40, 40, DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(40)); + break; + } + builder.addTemporalMarker(.2, robot.getArm()::armSecondaryScore); + builder.addTemporalMarker(.2,robot.getSlides()::slideAutoStacks); + builder.addTemporalMarker(.2, robot.getWrist()::wristScore); + this.robot.getDrive().followTrajectorySequence(builder.build()); + } + + protected void backTruss(){ + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(TO_BOARD); + builder.lineToLinearHeading(READY_TRUSS, + MecanumDrive.getVelocityConstraint(80, Math.toRadians(80), DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(80)); + builder.lineToLinearHeading(GET_STACK); + switch (randomization){ + case "MID": + builder.lineToLinearHeading(PICKUP_STACK_MID, + MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(20)); + break; + case "RIGHT": + builder.lineToLinearHeading(PICKUP_STACK, + MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(20)); + break; + default: + builder.lineToLinearHeading(PICKUP_STACK_LEFT, + MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(20)); + break; + } + + builder.addTemporalMarker(.3, robot.getArm()::armRest); + builder.addTemporalMarker(.3, robot.getWrist()::wristPickup); + builder.addTemporalMarker(.1, robot.getSlides()::slideDown); + builder.addTemporalMarker(1.5,robot.getClaw()::openStack); + builder.addTemporalMarker(1.5, robot.getArm()::armPickupStack); + this.robot.getDrive().followTrajectorySequence(builder.build()); + } + + protected void goBackstage() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(READY_TRUSS); + builder.lineToLinearHeading(TO_BOARD); + this.robot.getDrive().followTrajectorySequence(builder.build()); + } + + protected void scoreBoardStack(){ + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(READY_SCORESTACK); + builder.addTemporalMarker(.2, robot.getArm()::armSecondaryScore); + builder.addTemporalMarker(.2, robot.getSlides()::slideAutoStacks); + builder.addTemporalMarker(.2, robot.getWrist()::wristScore); + this.robot.getDrive().followTrajectorySequence(builder.build()); + } + + protected void scoreTest() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(new Pose2d(55, -41, Math.toRadians(0)), + MecanumDrive.getVelocityConstraint(20, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(20)); + builder.addTemporalMarker(.3,this::clawSlowOpen); + this.robot.getDrive().followTrajectorySequence(builder.build()); + } + + protected void clawSlowOpen() { + double currentPos = .81; + double targetPos = .73; + double delta = (targetPos - currentPos) / 100; + for (int i = 0; i < 100; i++) { + this.robot.getClaw().setPos(currentPos + (delta * i)); + sleep(30); + } + } + + + protected void park(){ + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + switch(parkLocation) { + case "LEFT": + builder.lineToLinearHeading(PARKLEFT); + builder.lineToLinearHeading(PARKLEFT2); + break; + case "RIGHT": + builder.lineToLinearHeading(PARK); + builder.lineToLinearHeading(PARK2); + break; + } + builder.addTemporalMarker(.1, robot.getArm()::armRest); + builder.addTemporalMarker(.1, robot.getWrist()::wristPickup); + builder.addTemporalMarker(.1, robot.getSlides()::slideDown); + this.robot.getDrive().followTrajectorySequence(builder.build()); + + } + + protected void parkLocation(){ + if (gamepad2.dpad_left) { + parkLocation="LEFT"; + } else if (gamepad2.dpad_right) { + parkLocation = "RIGHT"; + }} + + protected void delaySet(){ + if (gamepad2.dpad_up && delay<13000) { + delay+=1000; + sleep(100); + } else if (gamepad2.dpad_down && delay>0) { + delay-=1000; + sleep(100); + } + } + + + + @Override + public void runOpMode() throws InterruptedException { + this.telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + this.robot = new Robot().init(hardwareMap); + this.robot.getCamera().initTargetingCamera(); + this.initialPosition = new Pose2d(-34, -59.5, Math.toRadians(270)); + this.robot.getDrive().setPoseEstimate(initialPosition); + + // Do super fancy chinese shit + while (!this.isStarted()) { + parkLocation = "RIGHT"; + this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPosition()); + randomization = String.valueOf(this.robot.getCamera().getStartingPosition()); + this.telemetry.addData("Park Position", parkLocation); + this.telemetry.addData("Delay", delay); + this.telemetry.update(); + + } + scorePreloadOne(); + goBackToWhereYouCameFrom(); +// scoreBoard(); + score(); + this.robot.getClaw().open(); + backTruss(); + this.robot.getClaw().close(); + sleep(200); + goBackstage(); + scoreBoardStack(); + scoreTest(); + park(); + } + +}