limelight added to code

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

@@ -454,10 +454,6 @@ public class Red_V2 extends LinearOpMode {
 
             robot.hood.setPosition(hoodAuto);
 
-            double turretPID = servo.setTurrPos(turret_detectRed);
-            robot.turr1.setPower(turretPID);
-            robot.turr2.setPower(-turretPID);
-
             robot.transferServo.setPosition(transferServo_out);
 
             robot.spin1.setPower(spindexer_intakePos1);
@@ -465,6 +461,7 @@ public class Red_V2 extends LinearOpMode {
 
             aprilTag.update();
             TELE.addData("Velocity", velo);
+            TELE.addData("Turret Pos", servo.getTurrPos());
             TELE.update();
         }
 

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

@@ -0,0 +1,76 @@
+package org.firstinspires.ftc.teamcode.tests;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.hardware.limelightvision.LLResult;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.teamcode.utils.Robot;
+
+import java.util.List;
+
+public class LimelightTest extends LinearOpMode {
+    Robot robot;
+    MultipleTelemetry TELE;
+    public static int pipeline = 0; //0 is for test; 1, 2, and 3 are for obelisk; 4 is for blue track; 5 is for red track
+    public static int mode = 0; //0 for bare testing, 1 for obelisk, 2 for blue track, 3 for red track
+    int obeliskPipe = 1;
+    @Override
+    public void runOpMode() throws InterruptedException {
+        robot = new Robot(hardwareMap);
+        TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+        robot.limelight.pipelineSwitch(pipeline);
+
+        waitForStart();
+        if (isStopRequested()) return;
+        robot.limelight.start();
+        while (opModeIsActive()){
+            if (mode == 0){
+                robot.limelight.pipelineSwitch(pipeline);
+                LLResult result = robot.limelight.getLatestResult();
+                if (result != null) {
+                    if (result.isValid()) {
+                        TELE.addData("tx", result.getTx());
+                        TELE.addData("ty", result.getTy());
+                        TELE.update();
+                    }
+                }
+            } else if (mode == 1){
+                robot.limelight.pipelineSwitch(obeliskPipe);
+                LLResult result = robot.limelight.getLatestResult();
+                if (result != null && result.isValid()){
+                    TELE.addData("ID", obeliskPipe+20);
+                    TELE.update();
+                } else {
+                    if (obeliskPipe >= 3){
+                        obeliskPipe = 1;
+                    } else {
+                        obeliskPipe++;
+                    }
+                }
+            } else if (mode == 2){
+                robot.limelight.pipelineSwitch(4);
+                LLResult result = robot.limelight.getLatestResult();
+                if (result != null) {
+                    if (result.isValid()) {
+                        TELE.addData("tx", result.getTx());
+                        TELE.addData("ty", result.getTy());
+                        TELE.update();
+                    }
+                }
+            } else if (mode == 3){
+                robot.limelight.pipelineSwitch(5);
+                LLResult result = robot.limelight.getLatestResult();
+                if (result != null) {
+                    if (result.isValid()) {
+                        TELE.addData("tx", result.getTx());
+                        TELE.addData("ty", result.getTy());
+                        TELE.update();
+                    }
+                }
+            } else {
+                robot.limelight.pipelineSwitch(0);
+            }
+        }
+    }
+}

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

@@ -51,7 +51,7 @@ public class Flywheel {
             velo3 = velo2;
             velo2 = velo1;
 
-            currentPos = shooter1CurPos / 3072;
+            currentPos = shooter1CurPos / 2048;
             stamp = getTimeSeconds(); //getRuntime();
             velo1 = -60 * ((currentPos - initPos) / (stamp - stamp1));
             initPos = currentPos;

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

@@ -1,5 +1,6 @@
 package org.firstinspires.ftc.teamcode.utils;
 
+import com.qualcomm.hardware.limelightvision.Limelight3A;
 import com.qualcomm.hardware.rev.RevColorSensorV3;
 import com.qualcomm.robotcore.hardware.AnalogInput;
 import com.qualcomm.robotcore.hardware.CRServo;
@@ -81,6 +82,8 @@ public class Robot {
 
     public RevColorSensorV3 color3;
 
+    public Limelight3A limelight;
+
     public Robot(HardwareMap hardwareMap) {
 
         //Define components w/ hardware map
@@ -163,5 +166,7 @@ public class Robot {
         color2 = hardwareMap.get(RevColorSensorV3.class, "c2");
 
         color3 = hardwareMap.get(RevColorSensorV3.class, "c3");
+
+        limelight = hardwareMap.get(Limelight3A.class,"Limelight");
     }
 }