From 3e8e2c2b4bc733a98955e408bca66b8c256bbaeb Mon Sep 17 00:00:00 2001 From: KeshavAnandCode Date: Mon, 13 Oct 2025 18:12:03 -0500 Subject: [PATCH] fixed ll class --- .../ftc/teamcode/tests/LimelightTest.java | 1 + .../ftc/teamcode/utils/Robot.java | 18 ++++++++ .../utils/functions/DistanceToTarget.java | 8 ---- .../teamcode/utils/subsystems/Limelight.java | 42 +++++++++++++++++-- 4 files changed, 58 insertions(+), 11 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/functions/DistanceToTarget.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/LimelightTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/LimelightTest.java index 9973b8d..87932cd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/LimelightTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/LimelightTest.java @@ -61,6 +61,7 @@ public class LimelightTest extends LinearOpMode { TELE.update(); + } } 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 ad1f8b4..0a0050c 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 @@ -3,6 +3,7 @@ package org.firstinspires.ftc.teamcode.utils; import static org.firstinspires.ftc.teamcode.variables.HardwareConfig.*; import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.IMU; @@ -12,6 +13,8 @@ public class Robot { public Limelight3A limelight3A; + public IMU imu; + public Robot (HardwareMap hardwareMap) { //Define components w/ hardware map @@ -25,5 +28,20 @@ public class Robot { + + imu = hardwareMap.get(IMU.class, "imu"); + + RevHubOrientationOnRobot.LogoFacingDirection logoFacingDirection = RevHubOrientationOnRobot.LogoFacingDirection.UP; + + RevHubOrientationOnRobot.UsbFacingDirection usbFacingDirection = RevHubOrientationOnRobot.UsbFacingDirection.FORWARD; + + RevHubOrientationOnRobot orientationOnRobot = new RevHubOrientationOnRobot(logoFacingDirection, usbFacingDirection); + + imu.initialize(new IMU.Parameters(orientationOnRobot)); + + + + + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/functions/DistanceToTarget.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/functions/DistanceToTarget.java deleted file mode 100644 index 17b8c33..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/functions/DistanceToTarget.java +++ /dev/null @@ -1,8 +0,0 @@ -package org.firstinspires.ftc.teamcode.utils.functions; - -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; - -public class DistanceToTarget { - - -} 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 index 1704e2b..1fa9378 100644 --- 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 @@ -6,6 +6,7 @@ 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 com.qualcomm.robotcore.hardware.IMU; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; @@ -43,12 +44,16 @@ public class Limelight implements Subsystem { private double ty = 0.0; private double tync = 0.0; + private double ta = 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<>(); + private IMU imu; + public Limelight(Robot robot, MultipleTelemetry tele) { HardwareConfig.USING_LL= true; @@ -57,6 +62,10 @@ public class Limelight implements Subsystem { this.telemetry = tele; limelight.pipelineSwitch(1); + this.imu = robot.imu; + + this.imu.resetYaw(); + } @@ -66,15 +75,19 @@ public class Limelight implements Subsystem { public void setMode(String newMode) { this.mode = newMode; } + + /** ✅ MAIN UPDATE LOOP */ @Override public void update() { - limelight.updateRobotOrientation(); + result = limelight.getLatestResult(); status = limelight.getStatus(); + + if (result != null && (Objects.equals(status.getPipelineType(), "pipe_python") || result.isValid())){ // Refresh all cached values botpose = result.getBotpose(); @@ -86,6 +99,7 @@ public class Limelight implements Subsystem { txnc = result.getTxNC(); ty = result.getTy(); tync = result.getTyNC(); + ta = result.getTa(); barcodeResults = result.getBarcodeResults(); classifierResults = result.getClassifierResults(); detectorResults = result.getDetectorResults(); @@ -112,6 +126,7 @@ public class Limelight implements Subsystem { + if (result != null && result.isValid()) { telemetry.addData("LL Latency", getTotalLatency()); telemetry.addData("Capture Latency", getCaptureLatency()); @@ -122,7 +137,17 @@ public class Limelight implements Subsystem { telemetry.addData("txnc", getTxNC()); telemetry.addData("ty", getTy()); telemetry.addData("tync", getTyNC()); - telemetry.addData("Botpose", getBotPose().toString()); + telemetry.addData("ta", getTa()); + + + + + telemetry.addData("BotX", getBotX()); + + telemetry.addData("BotY", getBotY()); + + + if (Objects.equals(mode, "BR")) @@ -175,7 +200,16 @@ public class Limelight implements Subsystem { public double getTx() { return tx; } public double getTxNC() { return txnc; } public double getTy() { return ty; } - public double getTyNC() { return tync; } + public double getTyNC() { return tync;} + + public double getTa() {return ta;} + + public double getBotX() {return getBotPose().getPosition().x;} + + public double getBotY() {return getBotPose().getPosition().y;} + + + public List getBarcodeResults() { return barcodeResults; } public List getClassifierResults() { return classifierResults; } @@ -185,4 +219,6 @@ public class Limelight implements Subsystem { public LLStatus getStatus() { return status; } public LLResult getRawResult() { return result; } + + } \ No newline at end of file