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);