diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/ColorSensorTester.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/ColorSensorTester.java index a7feb1f..0054573 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/ColorSensorTester.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/ColorSensorTester.java @@ -4,12 +4,15 @@ package org.firstinspires.ftc.teamcode.utils; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DigitalChannel; @Config @TeleOp + +@Disabled public class ColorSensorTester extends LinearOpMode { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/ConfigureColorRangefinder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/ConfigureColorRangefinder.java index c3dd070..e19148a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/ConfigureColorRangefinder.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/ConfigureColorRangefinder.java @@ -6,7 +6,7 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.I2cDeviceSynchSimple; -//@Disabled +@Disabled @TeleOp public class ConfigureColorRangefinder extends LinearOpMode { @Override diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java index ae78075..d42c344 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java @@ -1,14 +1,19 @@ package org.firstinspires.ftc.teamcode.utils; +import com.qualcomm.hardware.limelightvision.Limelight3A; import com.qualcomm.robotcore.hardware.HardwareMap; public class Robot { //Initialize Public Components + public Limelight3A limelight3A; + public Robot (HardwareMap hardwareMap) { //Define components w/ hardware map + limelight3A = hardwareMap.get(Limelight3A.class, "limelight"); + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Subsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Subsystem.java new file mode 100644 index 0000000..52c117d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Subsystem.java @@ -0,0 +1,8 @@ +package org.firstinspires.ftc.teamcode.utils; + +public interface Subsystem { + + public void update(); + + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/subsystems/Limelight.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/subsystems/Limelight.java new file mode 100644 index 0000000..3bc3fa3 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/subsystems/Limelight.java @@ -0,0 +1,181 @@ +package org.firstinspires.ftc.teamcode.utils.subsystems; + + +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.LLResultTypes; +import com.qualcomm.hardware.limelightvision.LLStatus; +import com.qualcomm.hardware.limelightvision.Limelight3A; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; +import org.firstinspires.ftc.robotcore.external.navigation.Position; +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; +import org.firstinspires.ftc.teamcode.utils.Robot; +import org.firstinspires.ftc.teamcode.utils.Subsystem; + +import java.util.ArrayList; +import java.util.List; +import java.util.Objects; + +public class Limelight implements Subsystem { + + private final Limelight3A limelight; + private final MultipleTelemetry telemetry; + + private LLResult result; + private LLStatus status; + + private boolean telemetryOn = false; + private String mode = "AT"; + + // ✅ Internal cached data + private Pose3D botpose; + + private double captureLatency = 0.0; + private double targetingLatency = 0.0; + private double parseLatency = 0.0; + private double[] pythonOutput = new double[0]; + + private double tx = 0.0; + private double txnc = 0.0; + private double ty = 0.0; + private double tync = 0.0; + + private List barcodeResults = new ArrayList<>(); + private List classifierResults = new ArrayList<>(); + private List detectorResults = new ArrayList<>(); + private List fiducialResults = new ArrayList<>(); + private List colorResults = new ArrayList<>(); + + public Limelight(Robot robot, MultipleTelemetry tele) { + this.limelight = robot.limelight3A; + this.telemetry = tele; + limelight.pipelineSwitch(1); + + + } + + public void setPipeline(int pipeline) {limelight.pipelineSwitch(pipeline);} + + public void setTelemetryOn(boolean state) { telemetryOn = state; } + + public void setMode(String newMode) { this.mode = newMode; } + + /** ✅ MAIN UPDATE LOOP */ + @Override + public void update() { + result = limelight.getLatestResult(); + status = limelight.getStatus(); + + if (result != null && (Objects.equals(status.getPipelineType(), "pipe_python") || result.isValid())){ + // Refresh all cached values + botpose = result.getBotpose(); + captureLatency = result.getCaptureLatency(); + targetingLatency= result.getTargetingLatency(); + parseLatency = result.getParseLatency(); + pythonOutput = result.getPythonOutput(); + tx = result.getTx(); + txnc = result.getTxNC(); + ty = result.getTy(); + tync = result.getTyNC(); + barcodeResults = result.getBarcodeResults(); + classifierResults = result.getClassifierResults(); + detectorResults = result.getDetectorResults(); + fiducialResults = result.getFiducialResults(); + colorResults = result.getColorResults(); + } + + if (telemetryOn) telemetryUpdate(); + } + + /** ✅ Telemetry Output */ + private void telemetryUpdate() { +// ✅ Use getters instead of directly accessing 'status' or cached fields + telemetry.addData("Name", "%s", getStatus().getName()); + telemetry.addData("LL", "Temp: %.1fC, CPU: %.1f%%, FPS: %d", + getStatus().getTemp(), + getStatus().getCpu(), + (int) getStatus().getFps()); + telemetry.addData("Pipeline", "Index: %d, Type: %s", + getStatus().getPipelineIndex(), + getStatus().getPipelineType()); + telemetry.addData("ResultNull", result == null); + telemetry.addData("ResultValid", result.isValid()); + + + + if (result != null && result.isValid()) { + telemetry.addData("LL Latency", getTotalLatency()); + telemetry.addData("Capture Latency", getCaptureLatency()); + telemetry.addData("Targeting Latency", getTargetingLatency()); + telemetry.addData("Parse Latency", getParseLatency()); + telemetry.addData("PythonOutput", java.util.Arrays.toString(getPythonOutput())); + telemetry.addData("tx", getTx()); + telemetry.addData("txnc", getTxNC()); + telemetry.addData("ty", getTy()); + telemetry.addData("tync", getTyNC()); + telemetry.addData("Botpose", getBotPose().toString()); + + + if (Objects.equals(mode, "BR")) + for (LLResultTypes.BarcodeResult br : getBarcodeResults()) + telemetry.addData("Barcode", "Data: %s", br.getData()); + + if (Objects.equals(mode, "CL")) + for (LLResultTypes.ClassifierResult cr : getClassifierResults()) + telemetry.addData("Classifier", "Class: %s, Confidence: %.2f", + cr.getClassName(), cr.getConfidence()); + + if (Objects.equals(mode, "DE")) + for (LLResultTypes.DetectorResult dr : getDetectorResults()) + telemetry.addData("Detector", "Class: %s, Area: %.2f", + dr.getClassName(), dr.getTargetArea()); + + if (Objects.equals(mode, "FI")) + for (LLResultTypes.FiducialResult fr : getFiducialResults()) + telemetry.addData("Fiducial", "ID: %d, Family: %s, X: %.2f, Y: %.2f", + fr.getFiducialId(), fr.getFamily(), + fr.getTargetXDegrees(), fr.getTargetYDegrees()); + + if (Objects.equals(mode, "CO")) + for (LLResultTypes.ColorResult cr : getColorResults()) + telemetry.addData("Color", "X: %.2f, Y: %.2f", + cr.getTargetXDegrees(), cr.getTargetYDegrees()); + } else { + telemetry.addData("Limelight", "No data available"); + } + } + + // ✅ Getter methods (for use anywhere else in your code) + + public Pose3D getBotPose() { + if (botpose == null) { + botpose = new Pose3D( + new Position(), + new YawPitchRollAngles(AngleUnit.DEGREES, 0.0, 0.0, 0.0, 0L) + ); + } + return botpose; + } + + public double getCaptureLatency() { return captureLatency; } + public double getTargetingLatency() { return targetingLatency; } + public double getTotalLatency() { return captureLatency + targetingLatency; } + public double getParseLatency() { return parseLatency; } + public double[] getPythonOutput() { return pythonOutput; } + + public double getTx() { return tx; } + public double getTxNC() { return txnc; } + public double getTy() { return ty; } + public double getTyNC() { return tync; } + + public List getBarcodeResults() { return barcodeResults; } + public List getClassifierResults() { return classifierResults; } + public List getDetectorResults() { return detectorResults; } + public List getFiducialResults() { return fiducialResults; } + public List getColorResults() { return colorResults; } + + public LLStatus getStatus() { return status; } + public LLResult getRawResult() { return result; } +} \ No newline at end of file