yoooo

# 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;
-
-    // Ball color detection: 0 = no ball, 1 = green, 2 = purple
-    int b1 = 0, b2 = 0, b3 = 0;
-    boolean gpp = false, pgp = false, ppg = false;
+    boolean gpp = false;
+    boolean pgp = false;
+    boolean 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,35 +72,53 @@ public class AutoClose_V3 extends LinearOpMode {
 
     public Action Obelisk() {
         return new Action() {
+            int id = 0;
             @Override
             public boolean run(@NonNull TelemetryPacket telemetryPacket) {
-                limelightManager.update();
-                int id = limelightManager.detectFiducial();
+                LLResult result = robot.limelight.getLatestResult();
+                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 ?
-                            LimelightManager.LimelightMode.RED_GOAL :
-                            LimelightManager.LimelightMode.BLUE_GOAL;
-                    limelightManager.switchMode(mode);
-
-                    double turretTarget = redAlliance ? turret_redClose : turret_blueClose;
-                    double turretPID = servo.setTurrPos(turretTarget);
+                    if (redAlliance){
+                        robot.limelight.pipelineSwitch(3);
+                        double turretPID = servo.setTurrPos(turret_redClose);
                         robot.turr1.setPower(turretPID);
                         robot.turr2.setPower(-turretPID);
-                    return !servo.turretEqual(turretTarget);
+                        return !servo.turretEqual(turret_redClose);
+
+                    } else {
+                        robot.limelight.pipelineSwitch(2);
+                        double turretPID = servo.setTurrPos(turret_blueClose);
+                        robot.turr1.setPower(turretPID);
+                        robot.turr2.setPower(-turretPID);
+                        return !servo.turretEqual(turret_blueClose);
                     }
+                } else {
                     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));
-        servo = new Servos(hardwareMap);
-        limelightManager = new LimelightManager(hardwareMap, Robot.usingLimelight);
-        limelightManager.init();
-        limelightManager.switchMode(LimelightManager.LimelightMode.OBELISK_DETECTION);
+
+        TELE = new MultipleTelemetry(
+                telemetry, FtcDashboard.getInstance().getTelemetry()
+        );
+
+        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();
-        bearing = limelightManager.getBearing();
+        LLResult result = robot.limelight.getLatestResult();
+        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
-                else if (b2 == 2 && b1 == b3) sequence = sequences[2]; // 2,1,3
-                else if (b3 == 2 && b1 == b2) sequence = sequences[4]; // 3,1,2
-                else sequence = sequences[0];
+                if (b1 == 2 && b2 - b3 == 0) {
+                    sequence1();
+                    TELE.addLine("sequence1");
+                } 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];
-                else if (b2 == 2) sequence = sequences[2];
-                else if (b3 == 2) sequence = sequences[4];
-            } else sequence = sequences[0];
+                if (b1 == 2) {
+                    sequence1();
+                    TELE.addLine("sequence1");
+                } 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
-                else if (b2 == 2 && b1 == b3) sequence = sequences[0]; // 1,2,3
-                else if (b3 == 2 && b1 == b2) sequence = sequences[3]; // 2,3,1
-                else sequence = sequences[0];
+                if (b1 == 2 && b2 - b3 == 0) {
+                    sequence3();
+                    TELE.addLine("sequence3");
+                } 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];
-                else if (b2 == 2) sequence = sequences[0];
-                else if (b3 == 2) sequence = sequences[3];
-            } else sequence = sequences[2];
+                if (b1 == 2) {
+                    sequence3();
+                    TELE.addLine("sequence3");
+                } 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
-                else if (b2 == 2 && b1 == b3) sequence = sequences[5]; // 3,2,1
-                else if (b3 == 2 && b1 == b2) sequence = sequences[0]; // 1,2,3
-                else sequence = sequences[0];
+                if (b1 == 2 && b2 - b3 == 0) {
+                    sequence6();
+                    TELE.addLine("sequence6");
+                } 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];
-                else if (b2 == 2) sequence = sequences[5];
-                else if (b3 == 2) sequence = sequences[0];
-            } else sequence = sequences[4];
-        } else sequence = sequences[0];
-
-        executeShootingSequence(sequence);
+                if (b1 == 2) {
+                    sequence6();
+                    TELE.addLine("sequence6");
+                } else if (b2 == 2) {
+                    sequence5();
+                    TELE.addLine("sequence5");
+                } 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) {
-        double[] ballPositions = {
-            spindexer_outtakeBall1,
-            spindexer_outtakeBall2,
-            spindexer_outtakeBall3
-        };
-
-        for (double ball : sequence) {
+    public void sequence1() {
         Actions.runBlocking(
                 new SequentialAction(
-                            spindex(ballPositions[(int) ball - 1], AUTO_CLOSE_VEL),
+                        spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL),
+                        Shoot(AUTO_CLOSE_VEL),
+                        spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL),
+                        Shoot(AUTO_CLOSE_VEL),
+                        spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL),
+                        Shoot(AUTO_CLOSE_VEL)
+                )
+        );
+    }
+
+    public void sequence2() {
+        Actions.runBlocking(
+                new SequentialAction(
+                        spindex(spindexer_outtakeBall1, 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;
-
-    // Ball color detection: 0 = no ball, 1 = green, 2 = purple
-    int b1 = 0, b2 = 0, b3 = 0;
-    boolean gpp = false, pgp = false, ppg = false;
+    boolean gpp = false;
+    boolean pgp = false;
+    boolean 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,35 +72,53 @@ public class AutoFar_V1 extends LinearOpMode {
 
     public Action Obelisk() {
         return new Action() {
+            int id = 0;
             @Override
             public boolean run(@NonNull TelemetryPacket telemetryPacket) {
-                limelightManager.update();
-                int id = limelightManager.detectFiducial();
+                LLResult result = robot.limelight.getLatestResult();
+                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 ?
-                            LimelightManager.LimelightMode.RED_GOAL :
-                            LimelightManager.LimelightMode.BLUE_GOAL;
-                    limelightManager.switchMode(mode);
-
-                    double turretTarget = redAlliance ? turret_redFar : turret_blueFar;
-                    double turretPID = servo.setTurrPos(turretTarget);
+                    if (redAlliance){
+                        robot.limelight.pipelineSwitch(3);
+                        double turretPID = servo.setTurrPos(turret_redFar);
                         robot.turr1.setPower(turretPID);
                         robot.turr2.setPower(-turretPID);
-                    return !servo.turretEqual(turretTarget);
+                        return !servo.turretEqual(turret_redFar);
+
+                    } else {
+                        robot.limelight.pipelineSwitch(2);
+                        double turretPID = servo.setTurrPos(turret_blueFar);
+                        robot.turr1.setPower(turretPID);
+                        robot.turr2.setPower(-turretPID);
+                        return !servo.turretEqual(turret_blueFar);
                     }
+                } else {
                     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));
-        servo = new Servos(hardwareMap);
-        limelightManager = new LimelightManager(hardwareMap, Robot.usingLimelight);
-        limelightManager.init();
-        limelightManager.switchMode(LimelightManager.LimelightMode.OBELISK_DETECTION);
+
+        TELE = new MultipleTelemetry(
+                telemetry, FtcDashboard.getInstance().getTelemetry()
+        );
+
+        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();
-        bearing = limelightManager.getBearing();
+        LLResult result = robot.limelight.getLatestResult();
+        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/constants/ServoPositions.java

@@ -5,7 +5,7 @@ import com.acmerobotics.dashboard.config.Config;
 @Config
 public class ServoPositions {
 
-    public static double spindexer_intakePos1 = 0.34;
+    public static double spindexer_intakePos1 = 0.39;
 
     public static double spindexer_intakePos2 = 0.5;
 

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

@@ -6,8 +6,6 @@ import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.arcrobotics.ftclib.controller.PIDFController;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-import com.qualcomm.robotcore.hardware.CRServo;
-import com.qualcomm.robotcore.hardware.DcMotorSimple;
 
 import org.firstinspires.ftc.teamcode.utils.Robot;
 
@@ -33,7 +31,7 @@ public class PIDServoTest extends LinearOpMode {
 
         PIDFController controller = new PIDFController(p, i, d, f);
 
-        controller.setTolerance(0);
+        controller.setTolerance(0.001);
         robot = new Robot(hardwareMap);
 
         telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
@@ -46,12 +44,14 @@ public class PIDServoTest extends LinearOpMode {
             controller.setPIDF(p, i, d, f);
 
             if (mode == 0) {
-                pos = scalar * ((robot.turr1Pos.getVoltage() - restPos) / 3.3);
+                pos = (double) ((double)robot.turr1Pos.getCurrentPosition() /1024.0) * ((double) 44.0 /(double)77.0);
+
 
                 double pid = controller.calculate(pos, target);
 
                 robot.turr1.setPower(pid);
                 robot.turr2.setPower(-pid);
+
             } else if (mode == 1) {
                 pos = scalar * ((robot.spin1Pos.getVoltage() - restPos) / 3.3);
 
@@ -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);
@@ -71,4 +71,5 @@ public class PIDServoTest extends LinearOpMode {
         }
 
     }
+
 }

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/FlywheelV2.java

@@ -73,12 +73,18 @@ public class FlywheelV2 {
         targetVelocity = commandedVelocity;
         velo = getVelo(shooter1CurPos, shooter2CurPos);
         // Flywheel PID code here
-        if (targetVelocity - velo > 500) {
+        if (targetVelocity - velo > 4500) {
             powPID = 1.0;
-        } else if (velo - targetVelocity > 500) {
+        } else if (velo - targetVelocity > 4500) {
             powPID = 0.0;
         } else {
-            double feed = Math.log((668.39 / (targetVelocity + 591.96)) - 0.116) / -4.18;
+
+            double a = 2539.07863;
+            double c = 1967.6498;
+            double d = -0.289647;
+            double h = -1.1569;
+
+            double feed = Math.log10((a / (targetVelocity + c)) + d) / h;
 
             // --- PROPORTIONAL CORRECTION ---
             double error = targetVelocity - velo;

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());
-            TELE.addData("turret", servo.getTurrPos());
-            TELE.addData("spindexer voltage", robot.spin1Pos.getVoltage());
-            TELE.addData("hood voltage", robot.hoodPos.getVoltage());
+            // To check configuration of spindexer:
+            // Set "mode" to 1 and spindexPow to 0.1
+            // If the spindexer is turning clockwise, the servos are reversed. Swap the configuration of the two servos, DO NOT TOUCH THE ACTUAL CODE
+            // 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("Spin Equal", servo.spinEqual(spindexPos));
+            TELE.addData("turret voltage", robot.turr1Pos.getCurrentPosition());
+            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 AnalogInput turr1Pos;
-    public AnalogInput turr2Pos;
+    public DcMotorEx turr1Pos;
     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);
-
-        aprilTagProcessor = AprilTagProcessor.easyCreateWithDefaults();
-
-        webcam = hardwareMap.get(WebcamName.class, "Webcam 1");
+        transfer.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
 
         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

@@ -6,26 +6,24 @@ import com.qualcomm.robotcore.hardware.HardwareMap;
 
 @Config
 public class Servos {
-    Robot robot;
-
-    PIDFController spinPID;
-
-    PIDFController turretPID;
-
     //PID constants
     // TODO: get PIDF constants
-    public static double spinP = 3.4, spinI = 0, spinD = 0.075, spinF = 0.02;
-    public static double turrP = 4.0, turrI = 0.0, turrD = 0.0, turrF = 0.0;
-
+    public static double spinP = 3.3, spinI = 0, spinD = 0.1, spinF = 0.02;
+    public static double turrP = 1.1, turrI = 0.25, turrD = 0.0625, turrF = 0.01;
     public static double spin_scalar = 1.0086;
     public static double spin_restPos = 0.0;
     public static double turret_scalar = 1.009;
     public static double turret_restPos = 0.0;
+    Robot robot;
+    PIDFController spinPID;
+    PIDFController turretPID;
 
     public Servos(HardwareMap hardwareMap) {
         robot = new Robot(hardwareMap);
         spinPID = new PIDFController(spinP, spinI, spinD, spinF);
         turretPID = new PIDFController(turrP, turrI, turrD, turrF);
+
+        turretPID.setTolerance(0.001);
     }
 
     // In the code below, encoder = robot.servo.getVoltage()
@@ -33,6 +31,7 @@ public class Servos {
     public double getSpinPos() {
         return spin_scalar * ((robot.spin1Pos.getVoltage() - spin_restPos) / 3.3);
     }
+
     //TODO: PID warp so 0 and 1 are usable positions
     public double setSpinPos(double pos) {
         spinPID.setPIDF(spinP, spinI, spinD, spinF);
@@ -45,7 +44,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) {