moved ll to utils and got rid of spaghetti code for easier debugging

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,53 +68,35 @@ 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.turr2.setPower(-turretPID);
-                        return !servo.turretEqual(turret_blueClose);
+                    return !servo.turretEqual(turretTarget);
                 }
-                } else {
                 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) {
+        double[] ballPositions = {
+            spindexer_outtakeBall1,
+            spindexer_outtakeBall2,
+            spindexer_outtakeBall3
+        };
+
+        for (double ball : sequence) {
             Actions.runBlocking(
                     new SequentialAction(
-                        spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL),
+                            spindex(ballPositions[(int) ball - 1], 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/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;
+    }
+}