moved teleop LL stuff to LLManager - 3 instances were being created lol

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

@@ -18,19 +18,16 @@ 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 {
@@ -39,20 +36,19 @@ 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;
-    boolean gpp = false;
+    public static double holdTurrPow = 0.1;
-    boolean pgp = false;
+
-    boolean ppg = false;
+    // Ball color detection: 0 = no ball, 1 = green, 2 = purple
+    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() {
@@ -72,52 +68,34 @@ public class AutoClose_V3 extends LinearOpMode {
 
     public Action Obelisk() {
         return new Action() {
-            int id = 0;
             @Override
             public boolean run(@NonNull TelemetryPacket telemetryPacket) {
-                LLResult result = robot.limelight.getLatestResult();
+                limelightManager.update();
-                if (result != null && result.isValid()) {
+                int id = limelightManager.detectFiducial();
-                    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;
 
-                if (id == 21){
+                TELE.addData("Fiducial ID", id);
-                    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) {
-                    if (redAlliance){
+                    LimelightManager.LimelightMode mode = redAlliance ?
-                        robot.limelight.pipelineSwitch(3);
+                            LimelightManager.LimelightMode.RED_GOAL :
-                        double turretPID = servo.setTurrPos(turret_redClose);
+                            LimelightManager.LimelightMode.BLUE_GOAL;
-                        robot.turr1.setPower(turretPID);
+                    limelightManager.switchMode(mode);
-                        robot.turr2.setPower(-turretPID);
-                        return !servo.turretEqual(turret_redClose);
 
-                    } else {
+                    double turretTarget = redAlliance ? turret_redClose : turret_blueClose;
-                        robot.limelight.pipelineSwitch(2);
+                    double turretPID = servo.setTurrPos(turretTarget);
-                        double turretPID = servo.setTurrPos(turret_blueClose);
+                    robot.turr1.setPower(turretPID);
-                        robot.turr1.setPower(turretPID);
+                    robot.turr2.setPower(-turretPID);
-                        robot.turr2.setPower(-turretPID);
+                    return !servo.turretEqual(turretTarget);
-                        return !servo.turretEqual(turret_blueClose);
-                    }
-                } else {
-                    return true;
                 }
+                return true;
             }
         };
     }
@@ -361,21 +339,14 @@ 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());
-        TELE = new MultipleTelemetry(
+        drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
-                telemetry, FtcDashboard.getInstance().getTelemetry()
+        servo = new Servos(hardwareMap);
-        );
+        limelightManager = new LimelightManager(hardwareMap, Robot.usingLimelight);
-
+        limelightManager.init();
-        drive = new MecanumDrive(hardwareMap, new Pose2d(
+        limelightManager.switchMode(LimelightManager.LimelightMode.OBELISK_DETECTION);
-                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);
@@ -559,12 +530,8 @@ public class AutoClose_V3 extends LinearOpMode {
     }
     //TODO: adjust this according to Teleop numbers
     public void detectTag() {
-        LLResult result = robot.limelight.getLatestResult();
+        limelightManager.update();
-        if (result != null) {
+        bearing = limelightManager.getBearing();
-            if (result.isValid()) {
-                bearing = result.getTx();
-            }
-        }
         double turretPos = servo.getTurrPos() - (bearing / 1300);
         double turretPID = servo.setTurrPos(turretPos);
         robot.turr1.setPower(turretPID);
@@ -573,179 +540,67 @@ 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 == 0) {
+                if (b1 == 2 && b2 == b3) sequence = sequences[0];      // 1,2,3
-                    sequence1();
+                else if (b2 == 2 && b1 == b3) sequence = sequences[2]; // 2,1,3
-                    TELE.addLine("sequence1");
+                else if (b3 == 2 && b1 == b2) sequence = sequences[4]; // 3,1,2
-                } else if (b2 == 2 && b1 - b3 == 0) {
+                else sequence = sequences[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) {
+                if (b1 == 2) sequence = sequences[0];
-                    sequence1();
+                else if (b2 == 2) sequence = sequences[2];
-                    TELE.addLine("sequence1");
+                else if (b3 == 2) sequence = sequences[4];
-                } else if (b2 == 2) {
+            } else sequence = sequences[0];
-                    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 == 0) {
+                if (b1 == 2 && b2 == b3) sequence = sequences[2];      // 2,1,3
-                    sequence3();
+                else if (b2 == 2 && b1 == b3) sequence = sequences[0]; // 1,2,3
-                    TELE.addLine("sequence3");
+                else if (b3 == 2 && b1 == b2) sequence = sequences[3]; // 2,3,1
-                } else if (b2 == 2 && b1 - b3 == 0) {
+                else sequence = sequences[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) {
+                if (b1 == 2) sequence = sequences[2];
-                    sequence3();
+                else if (b2 == 2) sequence = sequences[0];
-                    TELE.addLine("sequence3");
+                else if (b3 == 2) sequence = sequences[3];
-                } else if (b2 == 2) {
+            } else sequence = sequences[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 == 0) {
+                if (b1 == 2 && b2 == b3) sequence = sequences[4];      // 3,1,2
-                    sequence6();
+                else if (b2 == 2 && b1 == b3) sequence = sequences[5]; // 3,2,1
-                    TELE.addLine("sequence6");
+                else if (b3 == 2 && b1 == b2) sequence = sequences[0]; // 1,2,3
-                } else if (b2 == 2 && b1 - b3 == 0) {
+                else sequence = sequences[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) {
+                if (b1 == 2) sequence = sequences[4];
-                    sequence6();
+                else if (b2 == 2) sequence = sequences[5];
-                    TELE.addLine("sequence6");
+                else if (b3 == 2) sequence = sequences[0];
-                } else if (b2 == 2) {
+            } else sequence = sequences[4];
-                    sequence5();
+        } else sequence = sequences[0];
-                    TELE.addLine("sequence5");
+
-                } else if (b3 == 2) {
+        executeShootingSequence(sequence);
-                    sequence1();
-                    TELE.addLine("sequence1");
-                }
-            } else {
-                sequence6();
-                TELE.addLine("sequence6");
-            }
-        } else {
-            sequence1();
-            TELE.addLine("sequence1");
-        }
         TELE.update();
     }
 
-    public void sequence1() {
+    private void executeShootingSequence(double[] sequence) {
-        Actions.runBlocking(
+        double[] ballPositions = {
-                new SequentialAction(
+            spindexer_outtakeBall1,
-                        spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL),
+            spindexer_outtakeBall2,
-                        Shoot(AUTO_CLOSE_VEL),
+            spindexer_outtakeBall3
-                        spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL),
+        };
-                        Shoot(AUTO_CLOSE_VEL),
-                        spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL),
-                        Shoot(AUTO_CLOSE_VEL)
-                )
-        );
-    }
 
-    public void sequence2() {
+        for (double ball : sequence) {
-        Actions.runBlocking(
+            Actions.runBlocking(
-                new SequentialAction(
+                    new SequentialAction(
-                        spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL),
+                            spindex(ballPositions[(int) ball - 1], 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,19 +18,16 @@ 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 {
@@ -39,20 +36,19 @@ 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;
-    boolean gpp = false;
+    public static double holdTurrPow = 0.1;
-    boolean pgp = false;
+
-    boolean ppg = false;
+    // Ball color detection: 0 = no ball, 1 = green, 2 = purple
+    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() {
@@ -72,52 +68,34 @@ public class AutoFar_V1 extends LinearOpMode {
 
     public Action Obelisk() {
         return new Action() {
-            int id = 0;
             @Override
             public boolean run(@NonNull TelemetryPacket telemetryPacket) {
-                LLResult result = robot.limelight.getLatestResult();
+                limelightManager.update();
-                if (result != null && result.isValid()) {
+                int id = limelightManager.detectFiducial();
-                    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;
 
-                if (id == 21){
+                TELE.addData("Fiducial ID", id);
-                    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) {
-                    if (redAlliance){
+                    LimelightManager.LimelightMode mode = redAlliance ?
-                        robot.limelight.pipelineSwitch(3);
+                            LimelightManager.LimelightMode.RED_GOAL :
-                        double turretPID = servo.setTurrPos(turret_redFar);
+                            LimelightManager.LimelightMode.BLUE_GOAL;
-                        robot.turr1.setPower(turretPID);
+                    limelightManager.switchMode(mode);
-                        robot.turr2.setPower(-turretPID);
-                        return !servo.turretEqual(turret_redFar);
 
-                    } else {
+                    double turretTarget = redAlliance ? turret_redFar : turret_blueFar;
-                        robot.limelight.pipelineSwitch(2);
+                    double turretPID = servo.setTurrPos(turretTarget);
-                        double turretPID = servo.setTurrPos(turret_blueFar);
+                    robot.turr1.setPower(turretPID);
-                        robot.turr1.setPower(turretPID);
+                    robot.turr2.setPower(-turretPID);
-                        robot.turr2.setPower(-turretPID);
+                    return !servo.turretEqual(turretTarget);
-                        return !servo.turretEqual(turret_blueFar);
-                    }
-                } else {
-                    return true;
                 }
+                return true;
             }
         };
     }
@@ -363,19 +341,13 @@ 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());
-        TELE = new MultipleTelemetry(
+        drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
-                telemetry, FtcDashboard.getInstance().getTelemetry()
+        servo = new Servos(hardwareMap);
-        );
+        limelightManager = new LimelightManager(hardwareMap, Robot.usingLimelight);
-
+        limelightManager.init();
-        drive = new MecanumDrive(hardwareMap, new Pose2d(
+        limelightManager.switchMode(LimelightManager.LimelightMode.OBELISK_DETECTION);
-                0, 0, 0
-        ));
-
-        robot.limelight.pipelineSwitch(1);
-        robot.limelight.start();
 
         //TODO: add positions to develop auto
         
@@ -460,12 +432,8 @@ public class AutoFar_V1 extends LinearOpMode {
     }
     //TODO: adjust this according to Teleop numbers
     public void detectTag() {
-        LLResult result = robot.limelight.getLatestResult();
+        limelightManager.update();
-        if (result != null) {
+        bearing = limelightManager.getBearing();
-            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.39;
+    public static double spindexer_intakePos1 = 0.34;
 
     public static double spindexer_intakePos2 = 0.5;
 

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

@@ -6,6 +6,8 @@ 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;
 
@@ -31,7 +33,7 @@ public class PIDServoTest extends LinearOpMode {
 
         PIDFController controller = new PIDFController(p, i, d, f);
 
-        controller.setTolerance(0.001);
+        controller.setTolerance(0);
         robot = new Robot(hardwareMap);
 
         telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
@@ -44,14 +46,12 @@ public class PIDServoTest extends LinearOpMode {
             controller.setPIDF(p, i, d, f);
 
             if (mode == 0) {
-                pos = (double) ((double)robot.turr1Pos.getCurrentPosition() /1024.0) * ((double) 44.0 /(double)77.0);
+                pos = scalar * ((robot.turr1Pos.getVoltage() - restPos) / 3.3);
-
 
                 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.getCurrentPosition());
+            telemetry.addData("Turret Voltage", robot.turr1Pos.getVoltage());
             telemetry.addData("Spindex Voltage", robot.spin1Pos.getVoltage());
             telemetry.addData("target", target);
             telemetry.addData("Mode", mode);
@@ -71,5 +71,4 @@ 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.FlywheelV2;
+import org.firstinspires.ftc.teamcode.utils.Flywheel;
 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;
-    FlywheelV2 flywheel;
+    Flywheel 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 FlywheelV2();
+        flywheel = new Flywheel();
 
         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(), robot.shooter2.getCurrentPosition());
+                double powPID = flywheel.manageFlywheel((int) parameter, robot.shooter1.getCurrentPosition());
                 rightShooter.setPower(powPID);
                 leftShooter.setPower(powPID);
                 TELE.addData("PIDPower", powPID);
@@ -71,9 +71,7 @@ public class ShooterTest extends LinearOpMode {
             } else {
                 robot.transferServo.setPosition(transferServo_out);
             }
-            TELE.addData("Velocity", flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()));
+            TELE.addData("Velocity", flywheel.getVelo());
-            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,18 +73,12 @@ public class FlywheelV2 {
         targetVelocity = commandedVelocity;
         velo = getVelo(shooter1CurPos, shooter2CurPos);
         // Flywheel PID code here
-        if (targetVelocity - velo > 4500) {
+        if (targetVelocity - velo > 500) {
             powPID = 1.0;
-        } else if (velo - targetVelocity > 4500) {
+        } else if (velo - targetVelocity > 500) {
             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

@@ -0,0 +1,103 @@
+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,25 +61,13 @@ public class PositionalServoProgrammer extends LinearOpMode {
             if (hoodPos != 0.501){
                 robot.hood.setPosition(hoodPos);
             }
-            // To check configuration of spindexer:
+            TELE.addData("spindexer", servo.getSpinPos());
-            // Set "mode" to 1 and spindexPow to 0.1
+            TELE.addData("turret", servo.getTurrPos());
-            // 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("spindexer voltage", robot.spin1Pos.getVoltage());
-            // Do the previous test again to confirm
+            TELE.addData("hood voltage", robot.hoodPos.getVoltage());
-            // 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.getCurrentPosition());
+            TELE.addData("turret voltage", robot.turr1Pos.getVoltage());
-            TELE.addData("spindexer pow", robot.spin1.getPower());
+            TELE.addData("Spin Equal", servo.spinEqual(spindexPos));
             TELE.update();
         }
     }

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

@@ -7,6 +7,7 @@ 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;
 
@@ -27,16 +28,28 @@ 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 DcMotorEx turr1Pos;
+    public AnalogInput hoodPos;
+    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;
@@ -44,12 +57,10 @@ 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");
@@ -63,24 +74,30 @@ 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");
 
-        turr1Pos = intake; // Encoder of turret plugged in intake port
+        turr2Pos = hardwareMap.get(AnalogInput.class, "t2Pos");
 
-        //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");
@@ -92,6 +109,22 @@ 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");
@@ -99,7 +132,10 @@ 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");
 
@@ -109,9 +145,6 @@ 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,24 +6,26 @@ 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.3, spinI = 0, spinD = 0.1, spinF = 0.02;
+    public static double spinP = 3.4, spinI = 0, spinD = 0.075, spinF = 0.02;
-    public static double turrP = 1.1, turrI = 0.25, turrD = 0.0625, turrF = 0.01;
+    public static double turrP = 4.0, turrI = 0.0, turrD = 0.0, turrF = 0.0;
+
     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()
@@ -31,7 +33,6 @@ 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);
@@ -44,7 +45,7 @@ public class Servos {
     }
 
     public double getTurrPos() {
-        return robot.turr1Pos.getCurrentPosition();
+        return turret_scalar * ((robot.turr1Pos.getVoltage() - turret_restPos) / 3.3);
     }
 
     public double setTurrPos(double pos) {