added limelight

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12BallPedroPathing.java

@@ -337,6 +337,8 @@ public class Auto12BallPedroPathing extends LinearOpMode {
                 initializePoses();
                 follower.setPose(startPose);
                 buildPaths();
+//                turret.switchPipeline(Turret.PipelineMode.OBELISK);
+                robot.limelight.start();
                 sleep(2000);
 
 //                turret.setTurret(turrDefault);

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java

@@ -326,6 +326,8 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
                 initializePoses();
                 follower.setPose(startPose);
                 buildPaths();
+//                turret.switchPipeline(Turret.PipelineMode.OBELISK);
+                robot.limelight.start();
                 sleep(2000);
             }
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java

@@ -16,6 +16,8 @@ import org.firstinspires.ftc.teamcode.constants.Color;
 import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
 import org.firstinspires.ftc.teamcode.utilsv2.*;
 
+import static org.firstinspires.ftc.teamcode.utilsv2.Turret.limelightUsed;
+
 @TeleOp
 @Config
 public class TeleopV4 extends LinearOpMode {
@@ -60,6 +62,10 @@ public class TeleopV4 extends LinearOpMode {
         spindexerTransferIntake = new SpindexerTransferIntake(robot, TELE, commander);
         spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID);
 
+        turret.switchPipeline(Turret.PipelineMode.TRACKING);
+        robot.limelight.start();
+
+        limelightUsed = false;
 
         waitForStart();
 
@@ -112,6 +118,12 @@ public class TeleopV4 extends LinearOpMode {
                 );
             }
 
+            if (gamepad2.leftBumperWasPressed()){
+                limelightUsed = false;
+            } else if (gamepad2.rightBumperWasPressed()){
+                limelightUsed = true;
+            }
+
             if (gamepad1.dpad_down){
                 parkTilter.park();
             } else if (gamepad1.dpad_up) {

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java

@@ -3,6 +3,7 @@ package org.firstinspires.ftc.teamcode.tests;
 import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseH;
 import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseX;
 import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseY;
+import static org.firstinspires.ftc.teamcode.utilsv2.Turret.limelightUsed;
 
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
@@ -63,6 +64,11 @@ public class NewShooterTest extends LinearOpMode {
         spindexerTransferIntake = new SpindexerTransferIntake(robot, TELE, commander);
         spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID);
 
+        turret.switchPipeline(Turret.PipelineMode.TRACKING);
+        robot.limelight.start();
+
+        limelightUsed = true;
+
         waitForStart();
 
         if (isStopRequested()) return;
@@ -83,6 +89,12 @@ public class NewShooterTest extends LinearOpMode {
             shooter.update(robot.voltage.getVoltage());
             spindexerTransferIntake.update();
 
+            if (gamepad2.leftBumperWasPressed()){
+                limelightUsed = false;
+            } else if (gamepad2.rightBumperWasPressed()){
+                limelightUsed = true;
+            }
+
             SpindexerTransferIntake.RapidMode state = spindexerTransferIntake.getRapidState();
 
             if (gamepad1.leftBumperWasPressed() &&

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java

@@ -43,13 +43,13 @@ public class Shooter {
 
         if (this.red) {
             goalX = 144;
-            turretGoalX = 136;
+            turretGoalX = 140;
         } else {
             goalX = 0;
             turretGoalX = 8;
         }
         goalY = 144;
-        turretGoalY = 136;
+        turretGoalY = 132;
     }
 
     private double flywheelVelocity = 0.0;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java

@@ -11,7 +11,6 @@ import com.qualcomm.robotcore.util.Range;
 
 
 import org.firstinspires.ftc.teamcode.constants.Color;
-import org.firstinspires.ftc.teamcode.teleop.TeleopV3;
 
 import java.util.List;
 
@@ -25,7 +24,6 @@ public class Turret {
     private final double turretMax = 0.95;
     public static boolean limelightUsed = true;
     public static double B_PID_P = 0.08, B_PID_I = 0.0, B_PID_D = 0.007;
-    Limelight3A webcam;
     LLResult result;
     PIDController bearingPID;
     boolean bearingAligned = false;
@@ -49,6 +47,7 @@ public class Turret {
 
     public Turret(Robot rob) {
         this.robot = rob;
+        bearingPID = new PIDController(B_PID_P, B_PID_I, B_PID_D);
     }
 
     private double wrapAngle(double angle) {
@@ -58,7 +57,8 @@ public class Turret {
     }
 
     private void limelightRead() { // only for tracking purposes, not general reads
-        result = webcam.getLatestResult();
+        switchPipeline(PipelineMode.TRACKING);
+        result = robot.limelight.getLatestResult();
         tx = 1000;
         if (result != null) {
             if (result.isValid()) {
@@ -67,6 +67,29 @@ public class Turret {
         }
     }
 
+    public enum PipelineMode{
+        OBELISK,
+        TRACKING
+    }
+
+    private int prevPipeline = 0;
+    public void switchPipeline(PipelineMode pipelineMode){
+        int pipeline = 0;
+        if (pipelineMode == PipelineMode.OBELISK){
+            pipeline = 1;
+        } else if (pipelineMode == PipelineMode.TRACKING){
+            if (Color.redAlliance){
+                pipeline = 4;
+            } else {
+                pipeline = 2;
+            }
+        }
+        if (pipeline == 0){prevPipeline = 0;}
+        if (pipeline != prevPipeline){
+            robot.limelight.pipelineSwitch(pipeline);
+        }
+    }
+
     public void trackObelisk(double dx, double dy, double h) {
 
         double heading = wrapAngle(h);
@@ -95,7 +118,7 @@ public class Turret {
     }
 
     private int detectObelisk() {
-        robot.limelight.pipelineSwitch(1);
+        switchPipeline(PipelineMode.OBELISK);
         result = robot.limelight.getLatestResult();
         if (result != null && result.isValid()) {
             List<LLResultTypes.FiducialResult> fiducials = result.getFiducialResults();