limelight works

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

@@ -3,66 +3,75 @@ package org.firstinspires.ftc.teamcode.tests;
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.hardware.limelightvision.LLResult;
+import com.qualcomm.hardware.limelightvision.LLResultTypes;
+import com.qualcomm.hardware.limelightvision.Limelight3A;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
-import org.firstinspires.ftc.teamcode.variables.HardwareConfig;
-import org.firstinspires.ftc.teamcode.utils.Robot;
-import org.firstinspires.ftc.teamcode.utils.subsystems.Limelight;
+import java.util.List;
 
-@Autonomous
+//TODO: fix to get the apriltag that it is reading
 @Config
-
+@TeleOp
 public class LimelightTest extends LinearOpMode {
-
-    public static String MODE = "AT";
-    public static int pipe = 1;
-
-
     MultipleTelemetry TELE;
-
-
-
-
-
-
-    Robot robot;
-
+    public static int pipeline = 0; //0 is for test; 1 for obelisk; 2 is for blue track; 3 is for red track
+    public static int mode = 0; //0 for bare testing, 1 for obelisk, 2 for blue track, 3 for red track
     @Override
     public void runOpMode() throws InterruptedException {
-
-
-
-        HardwareConfig.USING_LL= true;
-
-
-        robot = new Robot(hardwareMap);
-
+        Limelight3A limelight = hardwareMap.get(Limelight3A.class, "Limelight");
         TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
-
-        Limelight limelight = new Limelight(robot, TELE);
-
-        limelight.setMode(MODE);
-
-        limelight.setTelemetryOn(true);
-
+        limelight.pipelineSwitch(pipeline);
         waitForStart();
-
-        if(isStopRequested()) return;
-
-        while(opModeIsActive()){
-
-            limelight.setPipeline(pipe);
-
-            limelight.setMode(MODE);
-
-            limelight.update();
-
+        if (isStopRequested()) return;
+        limelight.start();
+        while (opModeIsActive()){
+            if (mode == 0){
+                limelight.pipelineSwitch(pipeline);
+                LLResult result = limelight.getLatestResult();
+                if (result != null) {
+                    if (result.isValid()) {
+                        TELE.addData("tx", result.getTx());
+                        TELE.addData("ty", result.getTy());
+                        TELE.update();
+                    }
+                }
+            } else if (mode == 1){
+                limelight.pipelineSwitch(1);
+                LLResult result = limelight.getLatestResult();
+                if (result != null && result.isValid()) {
+                    List<LLResultTypes.FiducialResult> fiducials = result.getFiducialResults();
+                    for (LLResultTypes.FiducialResult fiducial : fiducials) {
+                        int id = fiducial.getFiducialId();
+                        TELE.addData("ID", id);
                         TELE.update();
-
-
-
                     }
 
                 }
+            } else if (mode == 2){
+                limelight.pipelineSwitch(2);
+                LLResult result = limelight.getLatestResult();
+                if (result != null) {
+                    if (result.isValid()) {
+                        TELE.addData("tx", result.getTx());
+                        TELE.addData("ty", result.getTy());
+                        TELE.update();
+                    }
+                }
+            } else if (mode == 3){
+                limelight.pipelineSwitch(3);
+                LLResult result = limelight.getLatestResult();
+                if (result != null) {
+                    if (result.isValid()) {
+                        TELE.addData("tx", result.getTx());
+                        TELE.addData("ty", result.getTy());
+                        TELE.update();
+                    }
+                }
+            } else {
+                limelight.pipelineSwitch(0);
+            }
+        }
+    }
 }

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

@@ -5,7 +5,6 @@ import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
 
 import org.firstinspires.ftc.teamcode.utils.Flywheel;
@@ -18,28 +17,6 @@ public class ShooterTest extends LinearOpMode {
     public static double parameter = 0.0;
     // --- CONSTANTS YOU TUNE ---
     public static double hoodPos = 0.501;
-    double initPos = 0.0;
-    double stamp = 0.0;
-    double stamp1 = 0.0;
-    double ticker = 0.0;
-    double currentPos = 0.0;
-    double velo = 0.0;
-    double velo1 = 0.0;
-    double velo2 = 0.0;
-    double velo3 = 0.0;
-    double velo4 = 0.0;
-    double velo5 = 0.0;
-    double initPosq = 0.0;
-    double stampq = 0.0;
-    double stamp1q = 0.0;
-    double tickerq = 0.0;
-    double currentPosq = 0.0;
-    double veloq = 0.0;
-    double velo1q = 0.0;
-    double velo2q = 0.0;
-    double velo3q = 0.0;
-    double velo4q = 0.0;
-    double velo5q = 0.0;
     Robot robot;
     Flywheel flywheel;
 
@@ -64,40 +41,8 @@ public class ShooterTest extends LinearOpMode {
             if (mode == 0) {
                 rightShooter.setPower(parameter);
                 leftShooter.setPower(parameter);
-                leftShooter.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-                rightShooter.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-                if (ticker % 2 == 0) {
-                    velo5 = velo4;
-                    velo4 = velo3;
-                    velo3 = velo2;
-                    velo2 = velo1;
-
-                    currentPos = (double) leftShooter.getCurrentPosition() / 28;
-                    stamp = getRuntime();
-                    velo1 = 60 * ((currentPos - initPos) / (stamp - stamp1));
-                    initPos = currentPos;
-                    stamp1 = stamp;
-
-                    velo = (velo1 + velo2 + velo3 + velo4 + velo5) / 5;
-                }
-                if (tickerq % 2 == 0) {
-                    velo5q = velo4q;
-                    velo4q = velo3q;
-                    velo3q = velo2q;
-                    velo2q = velo1q;
-
-                    currentPosq = (double) rightShooter.getCurrentPosition() / 28;
-                    stampq = getRuntime();
-                    velo1q = 60 * ((currentPosq - initPosq) / (stampq - stamp1q));
-                    initPosq = currentPosq;
-                    stamp1q = stampq;
-
-                    veloq = (velo1q + velo2q + velo3q + velo4q + velo5q) / 5;
-                }
             } else if (mode == 1) {
-                leftShooter.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-                rightShooter.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-                double powPID = flywheel.manageFlywheel((int) parameter, robot.shooter1.getCurrentPosition());
+                double powPID = flywheel.manageFlywheel((int) parameter, leftShooter.getCurrentPosition(), rightShooter.getCurrentPosition());
                 rightShooter.setPower(powPID);
                 leftShooter.setPower(powPID);
                 TELE.addData("PIDPower", powPID);
@@ -107,9 +52,9 @@ public class ShooterTest extends LinearOpMode {
                 robot.hood.setPosition(hoodPos);
             }
 
-            TELE.addData("Velocity1", velo);
-            TELE.addData("Velocity2", veloq);
-            TELE.addData("Encoder Velocity", flywheel.getVelo());
+            TELE.addData("Used Velocity", flywheel.getVelo(leftShooter.getCurrentPosition(), rightShooter.getCurrentPosition()));
+            TELE.addData("Velocity1", flywheel.getVelo1());
+            TELE.addData("Velocity2", flywheel.getVelo2());
             TELE.addData("Power", robot.shooter1.getPower());
             TELE.addData("Steady?", flywheel.getSteady());
             TELE.addData("Position1", robot.shooter1.getCurrentPosition()/28);

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

@@ -6,13 +6,17 @@ import com.acmerobotics.dashboard.config.Config;
 public class Flywheel {
     public static double kP = 0.001;           // small proportional gain (tune this)
     public static double maxStep = 0.06;         // prevents sudden jumps
-    double initPos = 0.0;
+    double initPos1 = 0.0;
+    double initPos2 = 0.0;
     double stamp = 0.0;
     double stamp1 = 0.0;
     double ticker = 0.0;
-    double currentPos = 0.0;
+    double currentPos1 = 0.0;
+    double currentPos2 = 0.0;
     double velo = 0.0;
     double velo1 = 0.0;
+    double velo1a = 0.0;
+    double velo1b = 0.0;
     double velo2 = 0.0;
     double velo3 = 0.0;
     double velo4 = 0.0;
@@ -20,27 +24,12 @@ public class Flywheel {
     double targetVelocity = 0.0;
     double powPID = 0.0;
     boolean steady = false;
-    public Flywheel () {
+
+    public Flywheel() {
         //robot = new Robot(hardwareMap);
     }
 
-    public double getVelo () {
-        return velo;
-    }
-
-    public boolean getSteady() {
-        return steady;
-    }
-
-    private double getTimeSeconds ()
-    {
-        return (double) System.currentTimeMillis()/1000.0;
-    }
-
-
-    public double manageFlywheel(int commandedVelocity, double shooter1CurPos) {
-        targetVelocity = commandedVelocity;
-
+    public double getVelo(double shooter1CurPos, double shooter2CurPos) {
         ticker++;
         if (ticker % 2 == 0) {
             velo5 = velo4;
@@ -48,18 +37,45 @@ public class Flywheel {
             velo3 = velo2;
             velo2 = velo1;
 
-            currentPos = shooter1CurPos / 2048;
+            currentPos1 = shooter1CurPos / 28;
+            currentPos2 = shooter2CurPos / 28;
             stamp = getTimeSeconds(); //getRuntime();
-            velo1 = 60 * ((currentPos - initPos) / (stamp - stamp1));
-            initPos = currentPos;
+            velo1a = 60 * ((currentPos1 - initPos1) / (stamp - stamp1));
+            velo1b = 60 * ((currentPos2 - initPos2) / (stamp - stamp1));
+            initPos1 = currentPos1;
+            initPos2 = currentPos2;
             stamp1 = stamp;
 
-            velo = (velo1 + velo2 + velo3 + velo4 + velo5) / 5;
+            if (velo1a < 200){
+                velo1 = velo1b;
+            } else if (velo1b < 200){
+                velo1 = velo1a;
+            } else {
+                velo1 = (velo1a + velo1b) / 2;
             }
-        // Flywheel control code here
+        }
+        return ((velo1 + velo2 + velo3 + velo4 + velo5) / 5);
+    }
+
+    public double getVelo1() { return (velo1a + velo2 + velo3 + velo4 + velo5) / 5; }
+
+    public double getVelo2() { return (velo1b + velo2 + velo3 + velo4 + velo5) / 5; }
+
+    public boolean getSteady() {
+        return steady;
+    }
+
+    private double getTimeSeconds() {
+        return (double) System.currentTimeMillis() / 1000.0;
+    }
+
+    public double manageFlywheel(int commandedVelocity, double shooter1CurPos, double shooter2CurPos) {
+        targetVelocity = commandedVelocity;
+        velo = getVelo(shooter1CurPos, shooter2CurPos);
+        // Flywheel PID code here
         if (targetVelocity - velo > 500) {
             powPID = 1.0;
-        } else if (velo - targetVelocity > 500){
+        } else if (velo - targetVelocity > 500) {
             powPID = 0.0;
         } else {
             double feed = Math.log((668.39 / (targetVelocity + 591.96)) - 0.116) / -4.18;
@@ -78,13 +94,11 @@ public class Flywheel {
             powPID = Math.max(0, Math.min(1, powPID));
         }
 
-        // really should be a running average of the last 5
         steady = (Math.abs(targetVelocity - velo) < 100.0);
 
         return powPID;
     }
 
-    public void update()
-    {
+    public void update() {
     }
 }