Merge branch 'danielv2'

# Conflicts:
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Blue_V2.java
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red_V2.java
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java

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

@@ -18,16 +18,19 @@ import com.acmerobotics.roadrunner.SequentialAction;
 import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
 import com.acmerobotics.roadrunner.Vector2d;
 import com.acmerobotics.roadrunner.ftc.Actions;
+import com.qualcomm.hardware.limelightvision.LLResult;
+import com.qualcomm.hardware.limelightvision.LLResultTypes;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
 import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
 import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
 import org.firstinspires.ftc.teamcode.utils.FlywheelV2;
-import org.firstinspires.ftc.teamcode.utils.LimelightManager;
 import org.firstinspires.ftc.teamcode.utils.Robot;
 import org.firstinspires.ftc.teamcode.utils.Servos;
 
+import java.util.List;
+
 @Config
 @Autonomous(preselectTeleOp = "TeleopV3")
 public class AutoClose_V3 extends LinearOpMode {
@@ -36,19 +39,20 @@ public class AutoClose_V3 extends LinearOpMode {
     MecanumDrive drive;
     FlywheelV2 flywheel;
     Servos servo;
-    LimelightManager limelightManager;
 
     double velo = 0.0;
     public static double intake1Time = 2.7;
     public static double intake2Time = 3.0;
     public static double colorDetect = 3.0;
-    public static double holdTurrPow = 0.1;
+    boolean gpp = false;
-
+    boolean pgp = false;
-    // Ball color detection: 0 = no ball, 1 = green, 2 = purple
+    boolean ppg = false;
-    int b1 = 0, b2 = 0, b3 = 0;
-    boolean gpp = false, pgp = false, ppg = false;
     double powPID = 0.0;
     double bearing = 0.0;
+    int b1 = 0; // 0 = no ball, 1 = green, 2 = purple
+    int b2 = 0;// 0 = no ball, 1 = green, 2 = purple
+    int b3 = 0;// 0 = no ball, 1 = green, 2 = purple
+    public static double holdTurrPow = 0.1; // power to hold turret in place
 
     public Action initShooter(int vel) {
         return new Action() {
@@ -68,34 +72,52 @@ public class AutoClose_V3 extends LinearOpMode {
 
     public Action Obelisk() {
         return new Action() {
+            int id = 0;
             @Override
             public boolean run(@NonNull TelemetryPacket telemetryPacket) {
-                limelightManager.update();
+                LLResult result = robot.limelight.getLatestResult();
-                int id = limelightManager.detectFiducial();
+                if (result != null && result.isValid()) {
+                    List<LLResultTypes.FiducialResult> fiducials = result.getFiducialResults();
+                    for (LLResultTypes.FiducialResult fiducial : fiducials) {
+                        id = fiducial.getFiducialId();
+                        TELE.addData("ID", id);
+                        TELE.update();
+                    }
 
-                if (id == 21) gpp = true;
+                }
-                else if (id == 22) pgp = true;
-                else if (id == 23) ppg = true;
 
-                TELE.addData("Fiducial ID", id);
+                if (id == 21){
+                    gpp = true;
+                } else if (id == 22){
+                    pgp = true;
+                } else if (id == 23){
+                    ppg = true;
+                }
+
+                TELE.addData("Velocity", velo);
                 TELE.addData("21", gpp);
                 TELE.addData("22", pgp);
                 TELE.addData("23", ppg);
                 TELE.update();
 
                 if (gpp || pgp || ppg) {
-                    LimelightManager.LimelightMode mode = redAlliance ?
+                    if (redAlliance){
-                            LimelightManager.LimelightMode.RED_GOAL :
+                        robot.limelight.pipelineSwitch(3);
-                            LimelightManager.LimelightMode.BLUE_GOAL;
+                        double turretPID = servo.setTurrPos(turret_redClose);
-                    limelightManager.switchMode(mode);
+                        robot.turr1.setPower(turretPID);
+                        robot.turr2.setPower(-turretPID);
+                        return !servo.turretEqual(turret_redClose);
 
-                    double turretTarget = redAlliance ? turret_redClose : turret_blueClose;
+                    } else {
-                    double turretPID = servo.setTurrPos(turretTarget);
+                        robot.limelight.pipelineSwitch(2);
-                    robot.turr1.setPower(turretPID);
+                        double turretPID = servo.setTurrPos(turret_blueClose);
-                    robot.turr2.setPower(-turretPID);
+                        robot.turr1.setPower(turretPID);
-                    return !servo.turretEqual(turretTarget);
+                        robot.turr2.setPower(-turretPID);
+                        return !servo.turretEqual(turret_blueClose);
+                    }
+                } else {
+                    return true;
                 }
-                return true;
             }
         };
     }
@@ -339,14 +361,21 @@ public class AutoClose_V3 extends LinearOpMode {
 
     @Override
     public void runOpMode() throws InterruptedException {
+
         robot = new Robot(hardwareMap);
+
         flywheel = new FlywheelV2();
-        TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+
-        drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
+        TELE = new MultipleTelemetry(
-        servo = new Servos(hardwareMap);
+                telemetry, FtcDashboard.getInstance().getTelemetry()
-        limelightManager = new LimelightManager(hardwareMap, Robot.usingLimelight);
+        );
-        limelightManager.init();
+
-        limelightManager.switchMode(LimelightManager.LimelightMode.OBELISK_DETECTION);
+        drive = new MecanumDrive(hardwareMap, new Pose2d(
+                0, 0, 0
+        ));
+
+        robot.limelight.pipelineSwitch(1);
+        robot.limelight.start();
 
         TrajectoryActionBuilder shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
                 .strafeToLinearHeading(new Vector2d(bx1, by1), bh1);
@@ -530,8 +559,12 @@ public class AutoClose_V3 extends LinearOpMode {
     }
     //TODO: adjust this according to Teleop numbers
     public void detectTag() {
-        limelightManager.update();
+        LLResult result = robot.limelight.getLatestResult();
-        bearing = limelightManager.getBearing();
+        if (result != null) {
+            if (result.isValid()) {
+                bearing = result.getTx();
+            }
+        }
         double turretPos = servo.getTurrPos() - (bearing / 1300);
         double turretPID = servo.setTurrPos(turretPos);
         robot.turr1.setPower(turretPID);
@@ -540,67 +573,179 @@ public class AutoClose_V3 extends LinearOpMode {
 
     public void shootingSequence() {
         TELE.addData("Velocity", velo);
-        
-        // Define sequences based on obelisk configuration
-        double[][] sequences = {
-            {1, 2, 3}, {1, 3, 2}, {2, 1, 3}, {2, 3, 1}, {3, 1, 2}, {3, 2, 1}
-        };
-
-        double[] sequence = null;
-        
         if (gpp) {
             if (b1 + b2 + b3 == 4) {
-                if (b1 == 2 && b2 == b3) sequence = sequences[0];      // 1,2,3
+                if (b1 == 2 && b2 - b3 == 0) {
-                else if (b2 == 2 && b1 == b3) sequence = sequences[2]; // 2,1,3
+                    sequence1();
-                else if (b3 == 2 && b1 == b2) sequence = sequences[4]; // 3,1,2
+                    TELE.addLine("sequence1");
-                else sequence = sequences[0];
+                } else if (b2 == 2 && b1 - b3 == 0) {
+                    sequence3();
+                    TELE.addLine("sequence3");
+                } else if (b3 == 2 && b1 - b2 == 0) {
+                    sequence6();
+                    TELE.addLine("sequence6");
+                } else {
+                    sequence1();
+                    TELE.addLine("sequence1");
+                }
             } else if (b1 + b2 + b3 >= 5) {
-                if (b1 == 2) sequence = sequences[0];
+                if (b1 == 2) {
-                else if (b2 == 2) sequence = sequences[2];
+                    sequence1();
-                else if (b3 == 2) sequence = sequences[4];
+                    TELE.addLine("sequence1");
-            } else sequence = sequences[0];
+                } else if (b2 == 2) {
+                    sequence3();
+                    TELE.addLine("sequence3");
+                } else if (b3 == 2) {
+                    sequence6();
+                    TELE.addLine("sequence6");
+                }
+            } else {
+                sequence1();
+                TELE.addLine("sequence1");
+            }
         } else if (pgp) {
             if (b1 + b2 + b3 == 4) {
-                if (b1 == 2 && b2 == b3) sequence = sequences[2];      // 2,1,3
+                if (b1 == 2 && b2 - b3 == 0) {
-                else if (b2 == 2 && b1 == b3) sequence = sequences[0]; // 1,2,3
+                    sequence3();
-                else if (b3 == 2 && b1 == b2) sequence = sequences[3]; // 2,3,1
+                    TELE.addLine("sequence3");
-                else sequence = sequences[0];
+                } else if (b2 == 2 && b1 - b3 == 0) {
+                    sequence1();
+                    TELE.addLine("sequence1");
+                } else if (b3 == 2 && b1 - b2 == 0) {
+                    sequence4();
+                    TELE.addLine("sequence4");
+                } else {
+                    sequence1();
+                    TELE.addLine("sequence1");
+                }
             } else if (b1 + b2 + b3 >= 5) {
-                if (b1 == 2) sequence = sequences[2];
+                if (b1 == 2) {
-                else if (b2 == 2) sequence = sequences[0];
+                    sequence3();
-                else if (b3 == 2) sequence = sequences[3];
+                    TELE.addLine("sequence3");
-            } else sequence = sequences[2];
+                } else if (b2 == 2) {
+                    sequence1();
+                    TELE.addLine("sequence1");
+                } else if (b3 == 2) {
+                    sequence4();
+                    TELE.addLine("sequence4");
+                }
+            } else {
+                sequence3();
+                TELE.addLine("sequence3");
+            }
         } else if (ppg) {
             if (b1 + b2 + b3 == 4) {
-                if (b1 == 2 && b2 == b3) sequence = sequences[4];      // 3,1,2
+                if (b1 == 2 && b2 - b3 == 0) {
-                else if (b2 == 2 && b1 == b3) sequence = sequences[5]; // 3,2,1
+                    sequence6();
-                else if (b3 == 2 && b1 == b2) sequence = sequences[0]; // 1,2,3
+                    TELE.addLine("sequence6");
-                else sequence = sequences[0];
+                } else if (b2 == 2 && b1 - b3 == 0) {
+                    sequence5();
+                    TELE.addLine("sequence5");
+                } else if (b3 == 2 && b1 - b2 == 0) {
+                    sequence1();
+                    TELE.addLine("sequence1");
+                } else {
+                    sequence1();
+                    TELE.addLine("sequence1");
+                }
             } else if (b1 + b2 + b3 >= 5) {
-                if (b1 == 2) sequence = sequences[4];
+                if (b1 == 2) {
-                else if (b2 == 2) sequence = sequences[5];
+                    sequence6();
-                else if (b3 == 2) sequence = sequences[0];
+                    TELE.addLine("sequence6");
-            } else sequence = sequences[4];
+                } else if (b2 == 2) {
-        } else sequence = sequences[0];
+                    sequence5();
-
+                    TELE.addLine("sequence5");
-        executeShootingSequence(sequence);
+                } else if (b3 == 2) {
+                    sequence1();
+                    TELE.addLine("sequence1");
+                }
+            } else {
+                sequence6();
+                TELE.addLine("sequence6");
+            }
+        } else {
+            sequence1();
+            TELE.addLine("sequence1");
+        }
         TELE.update();
     }
 
-    private void executeShootingSequence(double[] sequence) {
+    public void sequence1() {
-        double[] ballPositions = {
+        Actions.runBlocking(
-            spindexer_outtakeBall1,
+                new SequentialAction(
-            spindexer_outtakeBall2,
+                        spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL),
-            spindexer_outtakeBall3
+                        Shoot(AUTO_CLOSE_VEL),
-        };
+                        spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL),
+                        Shoot(AUTO_CLOSE_VEL),
+                        spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL),
+                        Shoot(AUTO_CLOSE_VEL)
+                )
+        );
+    }
 
-        for (double ball : sequence) {
+    public void sequence2() {
-            Actions.runBlocking(
+        Actions.runBlocking(
-                    new SequentialAction(
+                new SequentialAction(
-                            spindex(ballPositions[(int) ball - 1], AUTO_CLOSE_VEL),
+                        spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL),
-                            Shoot(AUTO_CLOSE_VEL)
+                        Shoot(AUTO_CLOSE_VEL),
-                    )
+                        spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL),
-            );
+                        Shoot(AUTO_CLOSE_VEL),
-        }
+                        spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL),
+                        Shoot(AUTO_CLOSE_VEL)
+                )
+        );
+    }
+
+    public void sequence3() {
+        Actions.runBlocking(
+                new SequentialAction(
+                        spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL),
+                        Shoot(AUTO_CLOSE_VEL),
+                        spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL),
+                        Shoot(AUTO_CLOSE_VEL),
+                        spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL),
+
+                        Shoot(AUTO_CLOSE_VEL)
+                )
+        );
+    }
+
+    public void sequence4() {
+        Actions.runBlocking(
+                new SequentialAction(
+                        spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL),
+                        Shoot(AUTO_CLOSE_VEL),
+                        spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL),
+                        Shoot(AUTO_CLOSE_VEL),
+                        spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL),
+                        Shoot(AUTO_CLOSE_VEL)
+                )
+        );
+    }
+
+    public void sequence5() {
+        Actions.runBlocking(
+                new SequentialAction(
+                        spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL),
+                        Shoot(AUTO_CLOSE_VEL),
+                        spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL),
+                        Shoot(AUTO_CLOSE_VEL),
+                        spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL),
+                        Shoot(AUTO_CLOSE_VEL)
+                )
+        );
+    }
+
+    public void sequence6() {
+        Actions.runBlocking(
+                new SequentialAction(
+                        spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL),
+                        Shoot(AUTO_CLOSE_VEL),
+                        spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL),
+                        Shoot(AUTO_CLOSE_VEL),
+                        spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL),
+                        Shoot(AUTO_CLOSE_VEL)
+                )
+        );
     }
 }

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

@@ -18,16 +18,19 @@ import com.acmerobotics.roadrunner.SequentialAction;
 import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
 import com.acmerobotics.roadrunner.Vector2d;
 import com.acmerobotics.roadrunner.ftc.Actions;
+import com.qualcomm.hardware.limelightvision.LLResult;
+import com.qualcomm.hardware.limelightvision.LLResultTypes;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
 import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
 import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
 import org.firstinspires.ftc.teamcode.utils.FlywheelV2;
-import org.firstinspires.ftc.teamcode.utils.LimelightManager;
 import org.firstinspires.ftc.teamcode.utils.Robot;
 import org.firstinspires.ftc.teamcode.utils.Servos;
 
+import java.util.List;
+
 @Config
 @Autonomous(preselectTeleOp = "TeleopV3")
 public class AutoFar_V1 extends LinearOpMode {
@@ -36,19 +39,20 @@ public class AutoFar_V1 extends LinearOpMode {
     MecanumDrive drive;
     FlywheelV2 flywheel;
     Servos servo;
-    LimelightManager limelightManager;
 
     double velo = 0.0;
     public static double intake1Time = 2.7;
     public static double intake2Time = 3.0;
     public static double colorDetect = 3.0;
-    public static double holdTurrPow = 0.1;
+    boolean gpp = false;
-
+    boolean pgp = false;
-    // Ball color detection: 0 = no ball, 1 = green, 2 = purple
+    boolean ppg = false;
-    int b1 = 0, b2 = 0, b3 = 0;
-    boolean gpp = false, pgp = false, ppg = false;
     double powPID = 0.0;
     double bearing = 0.0;
+    int b1 = 0; // 0 = no ball, 1 = green, 2 = purple
+    int b2 = 0;// 0 = no ball, 1 = green, 2 = purple
+    int b3 = 0;// 0 = no ball, 1 = green, 2 = purple
+    public static double holdTurrPow = 0.1; // power to hold turret in place
 
     public Action initShooter(int vel) {
         return new Action() {
@@ -68,34 +72,52 @@ public class AutoFar_V1 extends LinearOpMode {
 
     public Action Obelisk() {
         return new Action() {
+            int id = 0;
             @Override
             public boolean run(@NonNull TelemetryPacket telemetryPacket) {
-                limelightManager.update();
+                LLResult result = robot.limelight.getLatestResult();
-                int id = limelightManager.detectFiducial();
+                if (result != null && result.isValid()) {
+                    List<LLResultTypes.FiducialResult> fiducials = result.getFiducialResults();
+                    for (LLResultTypes.FiducialResult fiducial : fiducials) {
+                        id = fiducial.getFiducialId();
+                        TELE.addData("ID", id);
+                        TELE.update();
+                    }
 
-                if (id == 21) gpp = true;
+                }
-                else if (id == 22) pgp = true;
-                else if (id == 23) ppg = true;
 
-                TELE.addData("Fiducial ID", id);
+                if (id == 21){
+                    gpp = true;
+                } else if (id == 22){
+                    pgp = true;
+                } else if (id == 23){
+                    ppg = true;
+                }
+
+                TELE.addData("Velocity", velo);
                 TELE.addData("21", gpp);
                 TELE.addData("22", pgp);
                 TELE.addData("23", ppg);
                 TELE.update();
 
                 if (gpp || pgp || ppg) {
-                    LimelightManager.LimelightMode mode = redAlliance ?
+                    if (redAlliance){
-                            LimelightManager.LimelightMode.RED_GOAL :
+                        robot.limelight.pipelineSwitch(3);
-                            LimelightManager.LimelightMode.BLUE_GOAL;
+                        double turretPID = servo.setTurrPos(turret_redFar);
-                    limelightManager.switchMode(mode);
+                        robot.turr1.setPower(turretPID);
+                        robot.turr2.setPower(-turretPID);
+                        return !servo.turretEqual(turret_redFar);
 
-                    double turretTarget = redAlliance ? turret_redFar : turret_blueFar;
+                    } else {
-                    double turretPID = servo.setTurrPos(turretTarget);
+                        robot.limelight.pipelineSwitch(2);
-                    robot.turr1.setPower(turretPID);
+                        double turretPID = servo.setTurrPos(turret_blueFar);
-                    robot.turr2.setPower(-turretPID);
+                        robot.turr1.setPower(turretPID);
-                    return !servo.turretEqual(turretTarget);
+                        robot.turr2.setPower(-turretPID);
+                        return !servo.turretEqual(turret_blueFar);
+                    }
+                } else {
+                    return true;
                 }
-                return true;
             }
         };
     }
@@ -341,13 +363,19 @@ public class AutoFar_V1 extends LinearOpMode {
     public void runOpMode() throws InterruptedException {
 
         robot = new Robot(hardwareMap);
+
         flywheel = new FlywheelV2();
-        TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+
-        drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
+        TELE = new MultipleTelemetry(
-        servo = new Servos(hardwareMap);
+                telemetry, FtcDashboard.getInstance().getTelemetry()
-        limelightManager = new LimelightManager(hardwareMap, Robot.usingLimelight);
+        );
-        limelightManager.init();
+
-        limelightManager.switchMode(LimelightManager.LimelightMode.OBELISK_DETECTION);
+        drive = new MecanumDrive(hardwareMap, new Pose2d(
+                0, 0, 0
+        ));
+
+        robot.limelight.pipelineSwitch(1);
+        robot.limelight.start();
 
         //TODO: add positions to develop auto
         
@@ -432,8 +460,12 @@ public class AutoFar_V1 extends LinearOpMode {
     }
     //TODO: adjust this according to Teleop numbers
     public void detectTag() {
-        limelightManager.update();
+        LLResult result = robot.limelight.getLatestResult();
-        bearing = limelightManager.getBearing();
+        if (result != null) {
+            if (result.isValid()) {
+                bearing = result.getTx();
+            }
+        }
         double turretPos = servo.getTurrPos() - (bearing / 1300);
         double turretPID = servo.setTurrPos(turretPos);
         robot.turr1.setPower(turretPID);

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

@@ -9,14 +9,14 @@ import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.acmerobotics.roadrunner.Pose2d;
+import com.qualcomm.hardware.limelightvision.LLResult;
 import com.qualcomm.hardware.lynx.LynxModule;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
 import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
 import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
-import org.firstinspires.ftc.teamcode.utils.Flywheel;
+import org.firstinspires.ftc.teamcode.utils.FlywheelV2;
-import org.firstinspires.ftc.teamcode.utils.LimelightManager;
 import org.firstinspires.ftc.teamcode.utils.Robot;
 import org.firstinspires.ftc.teamcode.utils.Servos;
 
@@ -29,9 +29,8 @@ public class TeleopV3 extends LinearOpMode {
     Robot robot;
     MultipleTelemetry TELE;
     Servos servo;
-    Flywheel flywheel;
+    FlywheelV2 flywheel;
     MecanumDrive drive;
-    LimelightManager limelightManager;
 
     public static double manualVel = 3000;
     public static double hoodDefaultPos = 0.5;
@@ -100,17 +99,17 @@ public class TeleopV3 extends LinearOpMode {
         robot = new Robot(hardwareMap);
         TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
         servo = new Servos(hardwareMap);
-        flywheel = new Flywheel();
+        flywheel = new FlywheelV2();
         drive = new MecanumDrive(hardwareMap, teleStart);
-        limelightManager = new LimelightManager(hardwareMap, Robot.usingLimelight);
-        limelightManager.init();
 
         if (redAlliance) {
-            limelightManager.switchMode(LimelightManager.LimelightMode.RED_GOAL);
+            robot.limelight.pipelineSwitch(3);
         } else {
-            limelightManager.switchMode(LimelightManager.LimelightMode.BLUE_GOAL);
+            robot.limelight.pipelineSwitch(2);
         }
 
+        robot.limelight.start();
+
         waitForStart();
         if (isStopRequested()) return;
         while (opModeIsActive()) {
@@ -257,7 +256,7 @@ public class TeleopV3 extends LinearOpMode {
 
             //SHOOTER:
 
-            double powPID = flywheel.manageFlywheel((int) vel, robot.shooter1.getCurrentPosition());
+            double powPID = flywheel.manageFlywheel((int) vel, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
 
             robot.shooter1.setPower(powPID);
             robot.shooter2.setPower(powPID);
@@ -305,26 +304,30 @@ public class TeleopV3 extends LinearOpMode {
             }
 
             if (y < 0.1 && y > -0.1 && x < 0.1 && x > -0.1 && rx < 0.1 && rx > -0.1) { //not moving
-                limelightManager.update();
+                double bearing;
-                double bearing = limelightManager.getBearing();
-                
-                if (limelightManager.getLatestResult() != null && limelightManager.getLatestResult().isValid()) {
-                    overrideTurr = true;
-                    turretPos = servo.getTurrPos() - (bearing / 1300);
 
-                    double bearingCorrection = bearing / 1300;
+                LLResult result = robot.limelight.getLatestResult();
+                if (result != null) {
+                    if (result.isValid()) {
+                        bearing = result.getTx();
+                        overrideTurr = true;
+                        turretPos = servo.getTurrPos() - (bearing / 1300);
 
-                    // deadband: ignore tiny noise
+                        double bearingCorrection = bearing / 1300;
-                    if (Math.abs(bearing) > 0.3 && camTicker < 8) {
+
-                        // only accumulate if bearing direction is consistent
+                        // deadband: ignore tiny noise
-                        if (Math.signum(bearingCorrection) == Math.signum(error) ||
+                        if (Math.abs(bearing) > 0.3 && camTicker < 8) {
-                            error == 0) {
+
-                            error += bearingCorrection;
+                            // only accumulate if bearing direction is consistent
+                            if (Math.signum(bearingCorrection) == Math.signum(error) ||
+                                error == 0) {
+                                error += bearingCorrection;
+                            }
                         }
+                        camTicker++;
+                        TELE.addData("tx", bearing);
+                        TELE.addData("ty", result.getTy());
                     }
-                    camTicker++;
-                    TELE.addData("tx", bearing);
-                    TELE.addData("ty", limelightManager.getLatestResult().getTy());
                 }
 
             } else {

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

@@ -46,7 +46,7 @@ public class PIDServoTest extends LinearOpMode {
             controller.setPIDF(p, i, d, f);
 
             if (mode == 0) {
-                pos = scalar * ((robot.turr1Pos.getVoltage() - restPos) / 3.3);
+                pos = robot.turr1Pos.getCurrentPosition();
 
                 double pid = controller.calculate(pos, target);
 
@@ -62,7 +62,7 @@ public class PIDServoTest extends LinearOpMode {
             }
 
             telemetry.addData("pos", pos);
-            telemetry.addData("Turret Voltage", robot.turr1Pos.getVoltage());
+            telemetry.addData("Turret Voltage", robot.turr1Pos.getCurrentPosition());
             telemetry.addData("Spindex Voltage", robot.spin1Pos.getVoltage());
             telemetry.addData("target", target);
             telemetry.addData("Mode", mode);

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

@@ -9,7 +9,7 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
 
-import org.firstinspires.ftc.teamcode.utils.Flywheel;
+import org.firstinspires.ftc.teamcode.utils.FlywheelV2;
 import org.firstinspires.ftc.teamcode.utils.Robot;
 
 @Config
@@ -26,7 +26,7 @@ public class ShooterTest extends LinearOpMode {
     public static double turretPos = 0.501;
     public static boolean shoot = false;
     Robot robot;
-    Flywheel flywheel;
+    FlywheelV2 flywheel;
 
     @Override
     public void runOpMode() throws InterruptedException {
@@ -34,7 +34,7 @@ public class ShooterTest extends LinearOpMode {
         robot = new Robot(hardwareMap);
         DcMotorEx leftShooter = robot.shooter1;
         DcMotorEx rightShooter = robot.shooter2;
-        flywheel = new Flywheel();
+        flywheel = new FlywheelV2();
 
         MultipleTelemetry TELE = new MultipleTelemetry(
                 telemetry, FtcDashboard.getInstance().getTelemetry()
@@ -50,7 +50,7 @@ public class ShooterTest extends LinearOpMode {
                 rightShooter.setPower(parameter);
                 leftShooter.setPower(parameter);
             } else if (mode == 1) {
-                double powPID = flywheel.manageFlywheel((int) parameter, robot.shooter1.getCurrentPosition());
+                double powPID = flywheel.manageFlywheel((int) parameter, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
                 rightShooter.setPower(powPID);
                 leftShooter.setPower(powPID);
                 TELE.addData("PIDPower", powPID);
@@ -71,7 +71,9 @@ public class ShooterTest extends LinearOpMode {
             } else {
                 robot.transferServo.setPosition(transferServo_out);
             }
-            TELE.addData("Velocity", flywheel.getVelo());
+            TELE.addData("Velocity", flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()));
+            TELE.addData("Velocity 1", flywheel.getVelo1());
+            TELE.addData("Velocity 2", flywheel.getVelo2());
             TELE.addData("Power", robot.shooter1.getPower());
             TELE.addData("Steady?", flywheel.getSteady());
             TELE.addData("Position", robot.shooter1.getCurrentPosition());

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/LimelightManager.java

@@ -1,103 +0,0 @@
-package org.firstinspires.ftc.teamcode.utils;
-
-import androidx.annotation.NonNull;
-
-import com.acmerobotics.dashboard.config.Config;
-import com.qualcomm.hardware.limelightvision.Limelight3A;
-import com.qualcomm.hardware.limelightvision.LLResult;
-import com.qualcomm.hardware.limelightvision.LLResultTypes;
-import com.qualcomm.robotcore.hardware.HardwareMap;
-
-import java.util.List;
-
-@Config
-public class LimelightManager {
-    private Limelight3A limelight;
-    private LLResult lastResult;
-    private int lastFiducialId = -1;
-    private double lastBearing = 0.0;
-
-    public static final int PIPELINE_DEFAULT = 1;
-    public static final int PIPELINE_BLUE_DETECTION = 2;
-    public static final int PIPELINE_RED_DETECTION = 3;
-
-    public enum LimelightMode {
-        OBELISK_DETECTION(PIPELINE_DEFAULT),
-        BLUE_GOAL(PIPELINE_BLUE_DETECTION),
-        RED_GOAL(PIPELINE_RED_DETECTION);
-
-        public final int pipeline;
-
-        LimelightMode(int pipeline) {
-            this.pipeline = pipeline;
-        }
-    }
-
-    public LimelightManager(HardwareMap hardwareMap, boolean enabled) {
-        if (enabled) {
-            this.limelight = hardwareMap.get(Limelight3A.class, "limelight");
-        }
-    }
-
-    public void init() {
-        if (limelight != null) {
-            limelight.start();
-        }
-    }
-
-    public void switchMode(LimelightMode mode) {
-        if (limelight != null) {
-            limelight.pipelineSwitch(mode.pipeline);
-        }
-    }
-
-    public void setPipeline(int pipeline) {
-        if (limelight != null) {
-            limelight.pipelineSwitch(pipeline);
-        }
-    }
-
-    public void update() {
-        if (limelight != null) {
-            lastResult = limelight.getLatestResult();
-            if (lastResult != null && lastResult.isValid()) {
-                lastBearing = lastResult.getTx();
-            }
-        }
-    }
-
-    public double getBearing() {
-        return lastBearing;
-    }
-
-    public int detectFiducial() {
-        if (lastResult != null && lastResult.isValid()) {
-            List<LLResultTypes.FiducialResult> fiducials = lastResult.getFiducialResults();
-            if (!fiducials.isEmpty()) {
-                lastFiducialId = fiducials.get(0).getFiducialId();
-                return lastFiducialId;
-            }
-        }
-        return -1;
-    }
-
-    public int getLastFiducialId() {
-        return lastFiducialId;
-    }
-
-    public boolean isFiducialDetected(int id) {
-        return lastFiducialId == id;
-    }
-
-    public LLResult getLatestResult() {
-        return lastResult;
-    }
-
-    public boolean isConnected() {
-        return limelight != null;
-    }
-
-    public Limelight3A getLimelight() {
-        return limelight;
-    }
-}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java

@@ -61,13 +61,25 @@ public class PositionalServoProgrammer extends LinearOpMode {
             if (hoodPos != 0.501){
                 robot.hood.setPosition(hoodPos);
             }
-            TELE.addData("spindexer", servo.getSpinPos());
+            // To check configuration of spindexer:
-            TELE.addData("turret", servo.getTurrPos());
+            // Set "mode" to 1 and spindexPow to 0.1
-            TELE.addData("spindexer voltage", robot.spin1Pos.getVoltage());
+            // If the spindexer is turning clockwise, the servos are reversed. Swap the configuration of the two servos, DO NOT TOUCH THE ACTUAL CODE
-            TELE.addData("hood voltage", robot.hoodPos.getVoltage());
+            // Do the previous test again to confirm
+            // Set "mode" to 0 but keep spindexPos at 0.501
+            // Manually turn the spindexer until "spindexer pos" is set close to 0
+            // Check each spindexer voltage:
+                // If "spindexer voltage 1" is closer to 0 than "spindexer voltage 2," then you are done!
+                // If "spindexer voltage 2" is closer to 0 than "spindexer voltage 1," swap the two spindexer analog inputs in the configuration, DO NOT TOUCH THE ACTUAL CODE
+            //TODO: @KeshavAnandCode do the above please
+
+            TELE.addData("spindexer pos", servo.getSpinPos());
+            TELE.addData("turret pos", servo.getTurrPos());
+            TELE.addData("spindexer voltage 1", robot.spin1Pos.getVoltage());
+            TELE.addData("spindexer voltage 2", robot.spin2Pos.getVoltage());
+            TELE.addData("hood pos", robot.hood.getPosition());
             TELE.addData("transferServo voltage", robot.transferServoPos.getVoltage());
-            TELE.addData("turret voltage", robot.turr1Pos.getVoltage());
+            TELE.addData("turret voltage", robot.turr1Pos.getCurrentPosition());
-            TELE.addData("Spin Equal", servo.spinEqual(spindexPos));
+            TELE.addData("spindexer pow", robot.spin1.getPower());
             TELE.update();
         }
     }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java

@@ -7,7 +7,6 @@ import com.qualcomm.robotcore.hardware.CRServo;
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
 import com.qualcomm.robotcore.hardware.DcMotorSimple;
-import com.qualcomm.robotcore.hardware.DigitalChannel;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 import com.qualcomm.robotcore.hardware.Servo;
 
@@ -28,28 +27,16 @@ public class Robot {
     public DcMotorEx shooter2;
     public Servo hood;
     public Servo transferServo;
-    public Servo rejecter;
     public CRServo turr1;
     public CRServo turr2;
     public CRServo spin1;
     public CRServo spin2;
-    public DigitalChannel pin0;
-    public DigitalChannel pin1;
-    public DigitalChannel pin2;
-    public DigitalChannel pin3;
-    public DigitalChannel pin4;
-    public DigitalChannel pin5;
-    public AnalogInput analogInput;
-    public AnalogInput analogInput2;
     public AnalogInput spin1Pos;
     public AnalogInput spin2Pos;
-    public AnalogInput hoodPos;
+    public DcMotorEx turr1Pos;
-    public AnalogInput turr1Pos;
-    public AnalogInput turr2Pos;
     public AnalogInput transferServoPos;
     public AprilTagProcessor aprilTagProcessor;
     public WebcamName webcam;
-    public DcMotorEx shooterEncoder;
     public RevColorSensorV3 color1;
     public RevColorSensorV3 color2;
     public RevColorSensorV3 color3;
@@ -57,10 +44,12 @@ public class Robot {
 
     public static boolean usingLimelight = true;
 
+    public static boolean usingCamera = true;
+
     public Robot(HardwareMap hardwareMap) {
 
         //Define components w/ hardware map
-
+        //TODO: fix the configuration of these - I trust you to figure it out yourself @KeshavAnandCode
         frontLeft = hardwareMap.get(DcMotorEx.class, "fl");
         frontRight = hardwareMap.get(DcMotorEx.class, "fr");
         backLeft = hardwareMap.get(DcMotorEx.class, "bl");
@@ -74,30 +63,24 @@ public class Robot {
         backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
 
         intake = hardwareMap.get(DcMotorEx.class, "intake");
-        rejecter = hardwareMap.get(Servo.class, "rejecter");
 
         shooter1 = hardwareMap.get(DcMotorEx.class, "shooter1");
 
         shooter2 = hardwareMap.get(DcMotorEx.class, "shooter2");
-
+        //TODO: figure out which shooter motor is reversed using ShooterTest and change it in code @KeshavAnandCode
         shooter1.setDirection(DcMotorSimple.Direction.REVERSE);
         shooter1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
         shooter2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
 
-        shooterEncoder = shooter1;
-
         hood = hardwareMap.get(Servo.class, "hood");
 
-        hoodPos = hardwareMap.get(AnalogInput.class, "hoodPos");
-
         turr1 = hardwareMap.get(CRServo.class, "t1");
 
-        turr1Pos = hardwareMap.get(AnalogInput.class, "t1Pos");
-
         turr2 = hardwareMap.get(CRServo.class, "t2");
 
-        turr2Pos = hardwareMap.get(AnalogInput.class, "t2Pos");
+        turr1Pos = intake; // Encoder of turret plugged in intake port
 
+        //TODO: check spindexer configuration (both servo and analog input) - check comments in PositionalServoProgrammer
         spin1 = hardwareMap.get(CRServo.class, "spin1");
 
         spin1Pos = hardwareMap.get(AnalogInput.class, "spin1Pos");
@@ -109,22 +92,6 @@ public class Robot {
         spin1.setDirection(DcMotorSimple.Direction.REVERSE);
         spin2.setDirection(DcMotorSimple.Direction.REVERSE);
 
-        pin0 = hardwareMap.get(DigitalChannel.class, "pin0");
-
-        pin1 = hardwareMap.get(DigitalChannel.class, "pin1");
-
-        pin2 = hardwareMap.get(DigitalChannel.class, "pin2");
-
-        pin3 = hardwareMap.get(DigitalChannel.class, "pin3");
-
-        pin4 = hardwareMap.get(DigitalChannel.class, "pin4");
-
-        pin5 = hardwareMap.get(DigitalChannel.class, "pin5");
-
-        analogInput = hardwareMap.get(AnalogInput.class, "analog");
-
-        analogInput2 = hardwareMap.get(AnalogInput.class, "analog2");
-
         transfer = hardwareMap.get(DcMotorEx.class, "transfer");
 
         transferServo = hardwareMap.get(Servo.class, "transferServo");
@@ -132,10 +99,7 @@ public class Robot {
         transferServoPos = hardwareMap.get(AnalogInput.class, "tSPos");
 
         transfer.setDirection(DcMotorSimple.Direction.REVERSE);
-
+        transfer.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-        aprilTagProcessor = AprilTagProcessor.easyCreateWithDefaults();
-
-        webcam = hardwareMap.get(WebcamName.class, "Webcam 1");
 
         color1 = hardwareMap.get(RevColorSensorV3.class, "c1");
 
@@ -145,6 +109,9 @@ public class Robot {
 
         if (usingLimelight){
             limelight = hardwareMap.get(Limelight3A.class, "limelight");
+        } else if (usingCamera){
+            webcam = hardwareMap.get(WebcamName.class, "Webcam 1");
+            aprilTagProcessor = AprilTagProcessor.easyCreateWithDefaults();
         }
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java

@@ -45,7 +45,7 @@ public class Servos {
     }
 
     public double getTurrPos() {
-        return turret_scalar * ((robot.turr1Pos.getVoltage() - turret_restPos) / 3.3);
+        return robot.turr1Pos.getCurrentPosition();
     }
 
     public double setTurrPos(double pos) {