Add auto randomization detection

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);

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);
+    }
+}

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);
+    }
+}

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;