Red and Blue auto works, Teleop works, everything works.

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/Auto.java

@@ -58,7 +58,7 @@ public class Auto extends LinearOpMode {
                 .build();
 
         this.boardOne = this.robot.getDrive().trajectoryBuilder(scoreOne.end())
-                .lineToLinearHeading(new Pose2d(72, -26, Math.toRadians(360)))
+                .lineToLinearHeading(new Pose2d(73, -26, Math.toRadians(360)))
                 .addTemporalMarker(.2, robot.getArm()::armAccurateScore)
                 .addTemporalMarker(.2, robot.getWrist()::wristScore)
                 .build();
@@ -74,7 +74,7 @@ public class Auto extends LinearOpMode {
                 .build();
 
         this.scoreTwo = this.robot.getDrive().trajectoryBuilder(preloadTwo.end())
-                .lineToLinearHeading(new Pose2d(72, -34, Math.toRadians(360)))
+                .lineToLinearHeading(new Pose2d(73, -34, Math.toRadians(360)))
                 .addTemporalMarker(.2, robot.getArm()::armAccurateScore)
                 .addTemporalMarker(.2, robot.getWrist()::wristScore)
                 .build();
@@ -89,7 +89,7 @@ public class Auto extends LinearOpMode {
                 .build();
 
         this.scoreThree = this.robot.getDrive().trajectoryBuilder(preloadThree.end())
-                .lineToLinearHeading(new Pose2d(72, -42, Math.toRadians(360)))
+                .lineToLinearHeading(new Pose2d(73, -42, Math.toRadians(360)))
                 .addTemporalMarker(.2, robot.getArm()::armAccurateScore)
                 .addTemporalMarker(.2, robot.getWrist()::wristScore)
                 .build();

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlue.java

@@ -0,0 +1,154 @@
+package org.firstinspires.ftc.teamcode.opmodes;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.teamcode.hardware.Robot;
+
+@Autonomous(name = "autoBlue")
+public class AutoBlue extends LinearOpMode {
+    protected Pose2d initialPosition;
+
+    protected Trajectory preloadOne;
+    protected Trajectory scoreOne;
+    protected Trajectory boardOne;
+    protected Trajectory backOffOne;
+
+
+    protected Trajectory preloadTwo;
+    protected Trajectory scoreTwo;
+    protected Trajectory backOffTwo;
+
+
+    protected Trajectory preloadThree;
+    protected Trajectory scoreThree;
+    protected Trajectory backOffThree;
+
+
+    protected Trajectory park1;
+    protected Trajectory park2;
+
+
+    private Robot robot;
+    private String randomization;
+    private int random;
+
+    @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();
+
+        //Trajectories
+        this.initialPosition = new Pose2d(-34, -59.5, Math.toRadians(270));
+        this.robot.getDrive().setPoseEstimate(initialPosition);
+
+        //Randomization One
+        this.preloadOne = this.robot.getDrive().trajectoryBuilder(initialPosition)
+                .lineToLinearHeading(new Pose2d(-40, -37.5, Math.toRadians(270)))
+                .build();
+
+        this.scoreOne = this.robot.getDrive().trajectoryBuilder(preloadOne.end())
+                .lineToLinearHeading(new Pose2d(-29, -30, Math.toRadians(180)))
+                .build();
+
+        this.boardOne = this.robot.getDrive().trajectoryBuilder(scoreOne.end())
+                .lineToLinearHeading(new Pose2d(-72, -26, Math.toRadians(180)))
+                .addTemporalMarker(.2, robot.getArm()::armAccurateScore)
+                .addTemporalMarker(.2, robot.getWrist()::wristScore)
+                .build();
+
+        this.backOffOne = this.robot.getDrive().trajectoryBuilder(boardOne.end())
+                .lineToLinearHeading(new Pose2d(-60, -26, Math.toRadians(180)))
+                .build();
+
+
+        //Randomization Two
+        this.preloadTwo = this.robot.getDrive().trajectoryBuilder(initialPosition)
+                .lineToLinearHeading(new Pose2d(-35, -27, Math.toRadians(270)))
+                .build();
+
+        this.scoreTwo = this.robot.getDrive().trajectoryBuilder(preloadTwo.end())
+                .lineToLinearHeading(new Pose2d(-70, -34, Math.toRadians(180)))
+                .addTemporalMarker(.2, robot.getArm()::armAccurateScore)
+                .addTemporalMarker(.2, robot.getWrist()::wristScore)
+                .build();
+
+        this.backOffTwo = this.robot.getDrive().trajectoryBuilder(scoreTwo.end())
+                .lineToLinearHeading(new Pose2d(-60, -34, Math.toRadians(180)))
+                .build();
+
+        //Randomization Three
+        this.preloadThree = this.robot.getDrive().trajectoryBuilder(initialPosition)
+                .lineToLinearHeading(new Pose2d(-42, -35, Math.toRadians(270)))
+                .build();
+
+        this.scoreThree = this.robot.getDrive().trajectoryBuilder(preloadThree.end())
+                .lineToLinearHeading(new Pose2d(-71.5, -41.3, Math.toRadians(180)))
+                .addTemporalMarker(.2, robot.getArm()::armAccurateScore)
+                .addTemporalMarker(.2, robot.getWrist()::wristScore)
+                .build();
+
+        this.backOffThree = this.robot.getDrive().trajectoryBuilder(scoreThree.end())
+                .lineToLinearHeading(new Pose2d(-60, -40, Math.toRadians(180)))
+                .build();
+
+        //Park
+        this.park1 = this.robot.getDrive().trajectoryBuilder(backOffTwo.end())
+                .lineToLinearHeading(new Pose2d(-65, -10, Math.toRadians(180)))
+                .addTemporalMarker(.3, robot.getArm()::armRest)
+                .addTemporalMarker(.3, robot.getWrist()::wristPickup)
+                .build();
+        this.park2 = this.robot.getDrive().trajectoryBuilder(park1.end())
+                .lineToLinearHeading(new Pose2d(-80, -10, Math.toRadians(180)))
+                .build();
+
+        // Do super fancy chinese shit
+        while (!this.isStarted()) {
+            this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPositionBlue());
+            randomization = String.valueOf(this.robot.getCamera().getStartingPositionBlue());
+            this.telemetry.update();
+        }
+
+        switch (randomization) {
+            case "RIGHT":
+                this.robot.getDrive().followTrajectory(preloadOne);
+                this.robot.getDrive().followTrajectory(scoreOne);
+                this.robot.getDrive().followTrajectory(boardOne);
+                this.robot.getClaw().open();
+                sleep(500);
+                this.robot.getDrive().followTrajectory(backOffOne);
+                sleep(500);
+                this.robot.getDrive().followTrajectory(park1);
+                this.robot.getDrive().followTrajectory(park2);
+                break;
+            case "CENTER":
+                this.robot.getDrive().followTrajectory(preloadTwo);
+                this.robot.getDrive().followTrajectory(scoreTwo);
+                this.robot.getClaw().open();
+                sleep(500);
+                this.robot.getDrive().followTrajectory(backOffTwo);
+                sleep(500);
+                this.robot.getDrive().followTrajectory(park1);
+                this.robot.getDrive().followTrajectory(park2);
+                break;
+            case "LEFT":
+                this.robot.getDrive().followTrajectory(preloadThree);
+                this.robot.getDrive().followTrajectory(scoreThree);
+                this.robot.getClaw().open();
+                sleep(500);
+                this.robot.getDrive().followTrajectory(backOffThree);
+                sleep(500);
+                this.robot.getDrive().followTrajectory(park1);
+                this.robot.getDrive().followTrajectory(park2);
+                break;
+        }
+
+
+    }
+
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/Camera.java

@@ -79,11 +79,49 @@ public class Camera {
             return StarterPosition.RIGHT;
         }
 
+
+
         return StarterPosition.UNKNOWN;
     }
 
+    public Detection getBlue() {
+        return targetingCameraInitialized ? targetingPipeline.getBlue() : INVALID_DETECTION;
+    }
+
+    public StarterPositionBlue getStartingPositionBlue() {
+        if (!targetingCameraInitialized) {
+            return StarterPositionBlue.UNKNOWN;
+        }
+
+        Detection detection = this.getBlue();
+        if (detection == null || !detection.isValid()) {
+            return StarterPositionBlue.UNKNOWN;
+        }
+        Point center = detection.getCenter();
+        if (center == null) {
+            return StarterPositionBlue.UNKNOWN;
+        }
+
+        double centerX = this.getBlue().getCenter().x + 50;
+        if (centerX < 33) {
+            return StarterPositionBlue.LEFT;
+        } else if (centerX < 66) {
+            return StarterPositionBlue.CENTER;
+        } else if (centerX < 100) {
+            return StarterPositionBlue.RIGHT;
+        }
+
+
+
+        return StarterPositionBlue.UNKNOWN;
+    }
+
     public enum StarterPosition {
         UNKNOWN, LEFT, CENTER, RIGHT
     }
 
+    public enum StarterPositionBlue {
+        UNKNOWN, LEFT, CENTER, RIGHT
+    }
+
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/ColorDetectionPipeline.java

@@ -1,8 +1,11 @@
 package org.firstinspires.ftc.teamcode.vision;
 
+import static org.firstinspires.ftc.teamcode.util.Configurables.FTC_BLUE_LOWER;
+import static org.firstinspires.ftc.teamcode.util.Configurables.FTC_BLUE_UPPER;
 import static org.firstinspires.ftc.teamcode.util.Configurables.FTC_RED_LOWER;
 import static org.firstinspires.ftc.teamcode.util.Configurables.FTC_RED_UPPER;
 import static org.firstinspires.ftc.teamcode.util.Constants.ANCHOR;
+import static org.firstinspires.ftc.teamcode.util.Constants.BLUE;
 import static org.firstinspires.ftc.teamcode.util.Constants.BLUR_SIZE;
 import static org.firstinspires.ftc.teamcode.util.Constants.ERODE_DILATE_ITERATIONS;
 import static org.firstinspires.ftc.teamcode.util.Constants.RED;
@@ -30,14 +33,26 @@ public class ColorDetectionPipeline extends OpenCvPipeline {
     Scalar redGoalLower2;
     Scalar redGoalUpper2;
 
+    Mat blueMask1 = new Mat();
+    Mat blueMask2 = new Mat();
+    Mat blueMask = new Mat();
+    Scalar blueGoalLower1;
+    Scalar blueGoalUpper1;
+    Scalar blueGoalLower2;
+    Scalar blueGoalUpper2;
+
     private Detection red;
+    private Detection blue;
+
 
     // Init
     @Override
     public void init(Mat input) {
         red = new Detection(input.size(), 0);
+        blue = new Detection(input.size(), 0);
     }
 
+
     // Process each frame that is received from the webcam
     @Override
     public Mat processFrame(Mat input) {
@@ -45,6 +60,7 @@ public class ColorDetectionPipeline extends OpenCvPipeline {
         Imgproc.cvtColor(blurred, hsv, Imgproc.COLOR_RGB2HSV);
 
         updateRed(input);
+        updateBlue(input);
 
         return input;
     }
@@ -74,7 +90,31 @@ public class ColorDetectionPipeline extends OpenCvPipeline {
         red.fill(input, RED);
     }
 
+    private void updateBlue(Mat input) {
+        // take pixels that are in the color range and put them into a mask, eroding and dilating them to remove white noise
+        blueGoalLower1 = new Scalar(FTC_BLUE_LOWER.getH(), FTC_BLUE_LOWER.getS(), FTC_BLUE_LOWER.getV());
+        blueGoalUpper1 = new Scalar(FTC_BLUE_UPPER.getH(), FTC_BLUE_UPPER.getS(), FTC_BLUE_UPPER.getV());
+        Core.inRange(hsv, blueGoalLower1, blueGoalUpper1, blueMask);
+        Imgproc.erode(blueMask, blueMask, STRUCTURING_ELEMENT, ANCHOR, ERODE_DILATE_ITERATIONS);
+        Imgproc.dilate(blueMask, blueMask, STRUCTURING_ELEMENT, ANCHOR, ERODE_DILATE_ITERATIONS);
+
+        ArrayList<MatOfPoint> contours = new ArrayList<>();
+        Imgproc.findContours(blueMask, contours, new Mat(), Imgproc.RETR_LIST, Imgproc.CHAIN_APPROX_SIMPLE);
+        for (int i = 0; i < contours.size(); i++) {
+            Detection newDetection = new Detection(input.size(), 0, 1f);
+            newDetection.setContour(contours.get(i));
+            newDetection.draw(input, BLUE);
+        }
+
+        blue.setContour(getLargestContour(contours));
+        blue.fill(input, BLUE);
+    }
+
     public Detection getRed() {
         return this.red;
     }
+
+    public Detection getBlue() {
+        return this.blue;
+    }
 }