diff --git a/Autonomous.java b/Autonomous.java index 1b52b0b..041c4c2 100644 --- a/Autonomous.java +++ b/Autonomous.java @@ -14,6 +14,9 @@ public class Autonomous extends LinearOpMode { protected static final boolean LEFT = false; protected static final boolean RIGHT = true; + protected static final boolean BLUE = false; + protected static final boolean RED = true; + private final boolean alliance; /** * Whether the robot is on the left or right side of the alliance station. */ @@ -32,12 +35,13 @@ public class Autonomous extends LinearOpMode { private Navigation navigation; - public Autonomous(boolean sideOfField) { + public Autonomous(boolean alliance, boolean sideOfField) { + this.alliance = alliance; this.sideOfField = sideOfField; } @Override - public void runOpMode() throws InterruptedException { + public void runOpMode() { TelemetryWrapper.init(telemetry, 20); TelemetryWrapper.setLine(1, "Autonomous " + "\t Initializing"); @@ -55,37 +59,50 @@ public void runOpMode() throws InterruptedException { secondPixel.init(hardwareMap, "secondPixel", 0, 0.3, false); firstPixel.setAction(true); secondPixel.setAction(true); -// randomizationWebcam = new PlaceDetectionWebcam(); -// randomizationWebcam.init(hardwareMap, "Blue.tflite"); -// int loc = 0; -// while (opModeInInit()){ -// loc = detectTape(randomizationWebcam); -// } -// randomizationWebcam.stop(); + + // Team element detection + randomizationWebcam = new PlaceDetectionWebcam(); + randomizationWebcam.init(hardwareMap, alliance ? "Red.tflite" : "Blue.tflite"); + int randomization = 0; + navigation = new Navigation(new double[]{12, 66}, 270, this, hardwareMap); pivot.startMoveToPos(false); while (opModeInInit()) { linearSlide.tickBeforeStart(); pivot.tickBeforeStart(); + randomization = detectTape(randomizationWebcam); TelemetryWrapper.setLine(7, "x: " + navigation.getCurrentPos()[0] + " y: " + navigation.getCurrentPos()[1]); TelemetryWrapper.setLine(8, "Gyro bearing: " + navigation.getGyroBearing()); } - - navigation.MoveToPosDirect(new double[]{12, 32}); + randomizationWebcam.stop(); + + navigation.moveToPosDirect(new double[]{12, 32}); + if (randomization == PlaceDetectionWebcam.CENTER) { + navigation.setBearing(90); + intakeWheel.setPower(-0.8); + sleep(1000); + intakeWheel.setPower(0); + } navigation.setBearing(0); while (opModeIsActive() && !navigation.tagCam.isDetecting) { navigation.tagCam.detectIter(navigation.getGyroBearing()); TelemetryWrapper.setLine(10, "Waiting for detection..."); } - navigation.MoveToPosDirect(new double[]{27, 32}); - navigation.setBearing(0); - intakeWheel.setPower(-0.8); - sleep(1000); - intakeWheel.setPower(0); - navigation.MoveToPosDirect(new double[]{45.5, 32}); + if (randomization == PlaceDetectionWebcam.RIGHT || randomization == PlaceDetectionWebcam.LEFT) { + if (randomization == PlaceDetectionWebcam.RIGHT) { + navigation.moveToPosDirect(new double[]{27, 32}); + } else { + navigation.moveToPosDirect(new double[]{10, 32}); + } + navigation.setBearing(0); + intakeWheel.setPower(-0.8); + sleep(1000); + intakeWheel.setPower(0); + } + navigation.moveToPosDirect(new double[]{45.5, 32}); navigation.setBearing(0); - linearSlide.startMoveToPos(1300); + linearSlide.startMoveToPos(1250); while (opModeIsActive() && !linearSlide.isFinished()) { linearSlide.tick(); } @@ -120,7 +137,7 @@ public void runOpMode() throws InterruptedException { // // Turn around // driveTrain.translate(0.8, 0, -32, 0, 10); // -// if (loc == PlaceDetectionWebcam.LEFT){ +// if (loc == PlaceDetectionWebcam.BLUE){ // driveTrain.translate(0.8, 0, 0, -90, 10); // double[] currentPos = aprilTagWebcam.detectIter(); // TelemetryWrapper.setLine(8, "Is detecting :" + aprilTagWebcam.isDetecting); diff --git a/BlueLeftAuto.java b/BlueLeftAuto.java new file mode 100644 index 0000000..f7aef07 --- /dev/null +++ b/BlueLeftAuto.java @@ -0,0 +1,8 @@ +package org.firstinspires.ftc.teamcode; + +@com.qualcomm.robotcore.eventloop.opmode.Autonomous(name = "BlueLeftAuto", group = "opmode") +public class BlueLeftAuto extends Autonomous { + public BlueLeftAuto() { + super(Autonomous.BLUE, Autonomous.LEFT); + } +} diff --git a/LeftAuto.java b/LeftAuto.java deleted file mode 100644 index 23f4458..0000000 --- a/LeftAuto.java +++ /dev/null @@ -1,8 +0,0 @@ -package org.firstinspires.ftc.teamcode; - -@com.qualcomm.robotcore.eventloop.opmode.Autonomous(name = "LeftAuto", group = "opmode") -public class LeftAuto extends Autonomous { - public LeftAuto() { - super(Autonomous.LEFT); - } -} diff --git a/RedRightAuto.java b/RedRightAuto.java new file mode 100644 index 0000000..9eb66c7 --- /dev/null +++ b/RedRightAuto.java @@ -0,0 +1,8 @@ +package org.firstinspires.ftc.teamcode; + +@com.qualcomm.robotcore.eventloop.opmode.Autonomous(name = "RedRightAuto", group = "opmode") +public class RedRightAuto extends Autonomous { + public RedRightAuto() { + super(Autonomous.RED, Autonomous.RIGHT); + } +} diff --git a/RightAuto.java b/RightAuto.java deleted file mode 100644 index 8fe3503..0000000 --- a/RightAuto.java +++ /dev/null @@ -1,8 +0,0 @@ -package org.firstinspires.ftc.teamcode; - -@com.qualcomm.robotcore.eventloop.opmode.Autonomous(name = "RightAuto", group = "opmode") -public class RightAuto extends Autonomous { - public RightAuto() { - super(Autonomous.RIGHT); - } -} diff --git a/modules/Navigation.java b/modules/Navigation.java index 2cd74ba..6bf053f 100644 --- a/modules/Navigation.java +++ b/modules/Navigation.java @@ -122,7 +122,7 @@ public void setBearing(double targetBearing) { } - public void MoveToPosDirect(double[] targetPos) { + public void moveToPosDirect(double[] targetPos) { double targetBearing = Math.toDegrees(Math.atan2(targetPos[1] - currentPos[1], targetPos[0] - currentPos[0])); setBearing(targetBearing); double[] lastEncoderPos = driveTrain.getEncPos(); @@ -202,7 +202,7 @@ public void calibrate() throws InterruptedException { TelemetryWrapper.setLine(13, "x: " + lastAprilTagPos[0] + "y: "+ lastAprilTagPos[1]); } - public void MoveToPosAtAngle(double[] targetPos, double angle) { + public void moveToPosAtAngle(double[] targetPos, double angle) { double directAngle = Math.toDegrees(Math.atan2(targetPos[1] - currentPos[1], targetPos[0] - currentPos[0])); directAngle = ((directAngle % 360) + 360) % 360; double deltaTheta = directAngle - angle;