From 359242c85df99846e1c7bbd30c6a17d49f321ccd Mon Sep 17 00:00:00 2001 From: Matthew Wheeler Date: Sat, 3 Jan 2026 12:30:46 -0800 Subject: [PATCH 1/6] Create a test harness that can communicate with Limelight and print detection data. --- build.gradle | 2 +- .../commands/test/VisionTestHarness.java | 54 +++++++++++++++++++ .../frc/robot/subsystems/VisionSubsystem.java | 32 +++++++++++ 3 files changed, 87 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/commands/test/VisionTestHarness.java create mode 100644 src/main/java/frc/robot/subsystems/VisionSubsystem.java diff --git a/build.gradle b/build.gradle index 471a256..a8d9e8d 100644 --- a/build.gradle +++ b/build.gradle @@ -8,7 +8,7 @@ java { targetCompatibility = JavaVersion.VERSION_17 } -def ROBOT_MAIN_CLASS = "frc.robot.Main" +def ROBOT_MAIN_CLASS = "frc.robot.commands.test.VisionTestHarness" // Define my targets (RoboRIO) and artifacts (deployable files) // This is added by GradleRIO's backing project DeployUtils. diff --git a/src/main/java/frc/robot/commands/test/VisionTestHarness.java b/src/main/java/frc/robot/commands/test/VisionTestHarness.java new file mode 100644 index 0000000..1400d2b --- /dev/null +++ b/src/main/java/frc/robot/commands/test/VisionTestHarness.java @@ -0,0 +1,54 @@ +package frc.robot.commands.test; + +import edu.wpi.first.networktables.NetworkTableEvent; +import edu.wpi.first.networktables.NetworkTableInstance; + +public class VisionTestHarness { + public static void main(String[] args) { + edu.wpi.first.hal.HAL.initialize(500, 0); + NetworkTableInstance nt = NetworkTableInstance.getDefault(); + + // Stop client mode and start as the Server + nt.stopClient(); + nt.startServer(); + + System.out.println("Laptop is now the NT Server. Waiting for Limelight..."); + + // 2025-style Connection Listener + nt.addConnectionListener(true, event -> { + if (event.is(NetworkTableEvent.Kind.kConnected)) { + System.out.println("[CONNECTED] Limelight has joined the table!"); + } + }); + + while (true) { + var table = nt.getTable("limelight"); + + // tv: 1 if a target is visible, 0 if not + boolean hasTarget = table.getEntry("tv").getDouble(0.0) == 1.0; + + if (hasTarget) { + // Horizontal offset (tx) and Vertical offset (ty) + double tx = table.getEntry("tx").getDouble(0.0); + double ty = table.getEntry("ty").getDouble(0.0); + + // tclass: The string label of the detected object (e.g., "cube", "cone") + String detectedClass = table.getEntry("tclass").getString("None"); + String rawJson = table.getEntry("json").getString(""); + if (!rawJson.isEmpty()) { + System.out.println("RAW DATA: " + rawJson); + } + + // ta: The target area (0% to 100% of the image) + double area = table.getEntry("ta").getDouble(0.0); + + System.out.printf("[DETECTED] Class: %s | TX: %.2f | TY: %.2f | Area: %.2f%%\n", + detectedClass, tx, ty, area); + } else { + System.out.println("Searching for objects..."); + } + + try { Thread.sleep(200); } catch (InterruptedException e) {} // 5Hz log rate + } + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java new file mode 100644 index 0000000..e225ade --- /dev/null +++ b/src/main/java/frc/robot/subsystems/VisionSubsystem.java @@ -0,0 +1,32 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.Limelights.LimelightHelpers; // Note your specific path + +public class VisionSubsystem extends SubsystemBase { + private final String llName = "limelight"; + + public VisionSubsystem() {} + + // Get the horizontal offset to the target + public double getTX() { + return LimelightHelpers.getTX(llName); + } + + // Get the vertical offset (useful for distance estimation) + public double getTY() { + return LimelightHelpers.getTY(llName); + } + + // Identify the YOLOv8 class currently being seen + public String getDetectedClass() { + return !LimelightHelpers.getNeuralClassID(llName).isEmpty() ? + LimelightHelpers.getLatestResults(llName).targetingResults.classifierResults.get(0).className : + "None"; + } + + @Override + public void periodic() { + // This runs 50 times a second on the robot + } +} \ No newline at end of file From b6f585baf6d0aee3dc443a81486f393111079ecc Mon Sep 17 00:00:00 2001 From: Matthew Wheeler Date: Sat, 3 Jan 2026 15:30:52 -0800 Subject: [PATCH 2/6] Update LimelightHelpers.java --- src/main/java/frc/robot/Robot.java | 4 +- .../Limelights/LimelightHelpers.java | 2115 +++++++++++------ 2 files changed, 1341 insertions(+), 778 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d303fb5..c670bc6 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -41,7 +41,7 @@ private void configureLimelights() { private void updateLimelightTelemetry() { for (String limelightName : Constants.LimelightConstants.limelightNames) { - boolean hasTarget = LimelightHelpers.hasTarget(limelightName); + boolean hasTarget = LimelightHelpers.getTV(limelightName); SmartDashboard.putBoolean(limelightName + "/HasTarget", hasTarget); if (hasTarget) { @@ -69,7 +69,7 @@ private void updateVisionMeasurement() { var omega = driveState.Speeds.omegaRadiansPerSecond; for (String limelightName : Constants.LimelightConstants.limelightNames) { - LimelightHelpers.setRobotOrientation( + LimelightHelpers.SetRobotOrientation( limelightName, heading, omega * 180.0 / Math.PI, diff --git a/src/main/java/frc/robot/subsystems/Limelights/LimelightHelpers.java b/src/main/java/frc/robot/subsystems/Limelights/LimelightHelpers.java index afdbd0b..582b0f3 100644 --- a/src/main/java/frc/robot/subsystems/Limelights/LimelightHelpers.java +++ b/src/main/java/frc/robot/subsystems/Limelights/LimelightHelpers.java @@ -1,442 +1,769 @@ +//LimelightHelpers v1.11 (REQUIRES LLOS 2025.0 OR LATER) + package frc.robot.subsystems.Limelights; +import edu.wpi.first.networktables.DoubleArrayEntry; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.TimestampedDoubleArray; +import frc.robot.subsystems.Limelights.LimelightHelpers.LimelightResults; +import frc.robot.subsystems.Limelights.LimelightHelpers.PoseEstimate; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.networktables.NetworkTableInstance; + +import java.io.IOException; +import java.net.HttpURLConnection; +import java.net.MalformedURLException; +import java.net.URL; +import java.util.Map; +import java.util.concurrent.CompletableFuture; + +import com.fasterxml.jackson.annotation.JsonFormat; +import com.fasterxml.jackson.annotation.JsonFormat.Shape; import com.fasterxml.jackson.annotation.JsonProperty; import com.fasterxml.jackson.core.JsonProcessingException; import com.fasterxml.jackson.databind.DeserializationFeature; import com.fasterxml.jackson.databind.ObjectMapper; - -import java.io.IOException; -import java.util.ArrayList; -import java.util.List; - -//TODO: compare to the origin wpilib official LimelightHelpers.java document +import java.util.concurrent.ConcurrentHashMap; /** - * LimelightHelpers v1.6.0 (Java) - * - * A utility class for interfacing with Limelight vision systems in FRC robots. - * Based on the official LimelightHelpers library. - * - * This is a single-file library - just copy this file into your robot package. + * LimelightHelpers provides static methods and classes for interfacing with Limelight vision cameras in FRC. + * This library supports all Limelight features including AprilTag tracking, Neural Networks, and standard color/retroreflective tracking. */ public class LimelightHelpers { + private static final Map doubleArrayEntries = new ConcurrentHashMap<>(); + /** - * Sanitizes the Limelight name. Returns "limelight" if name is empty or null. + * Represents a Color/Retroreflective Target Result extracted from JSON Output */ - public static String sanitizeName(String name) { - if (name == null || name.isEmpty()) { - return "limelight"; + public static class LimelightTarget_Retro { + + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; + + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; + + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; + + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() + { + return toPose3D(cameraPose_TargetSpace); + } + public Pose3d getRobotPose_FieldSpace() + { + return toPose3D(robotPose_FieldSpace); + } + public Pose3d getRobotPose_TargetSpace() + { + return toPose3D(robotPose_TargetSpace); + } + public Pose3d getTargetPose_CameraSpace() + { + return toPose3D(targetPose_CameraSpace); + } + public Pose3d getTargetPose_RobotSpace() + { + return toPose3D(targetPose_RobotSpace); } - return name; - } - /** - * Gets the NetworkTable for the specified Limelight. - */ - public static NetworkTable getLimelightNTTable(String tableName) { - return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); - } + public Pose2d getCameraPose_TargetSpace2D() + { + return toPose2D(cameraPose_TargetSpace); + } + public Pose2d getRobotPose_FieldSpace2D() + { + return toPose2D(robotPose_FieldSpace); + } + public Pose2d getRobotPose_TargetSpace2D() + { + return toPose2D(robotPose_TargetSpace); + } + public Pose2d getTargetPose_CameraSpace2D() + { + return toPose2D(targetPose_CameraSpace); + } + public Pose2d getTargetPose_RobotSpace2D() + { + return toPose2D(targetPose_RobotSpace); + } - /** - * Gets a specific NetworkTableEntry from the Limelight table. - */ - public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) { - return getLimelightNTTable(tableName).getEntry(entryName); - } + @JsonProperty("ta") + public double ta; - /** - * Gets a double value from the Limelight NetworkTable. - */ - public static double getLimelightNTDouble(String tableName, String entryName) { - return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0); - } + @JsonProperty("tx") + public double tx; - /** - * Gets a double array from the Limelight NetworkTable. - */ - public static double[] getLimelightNTDoubleArray(String tableName, String entryName) { - return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]); - } + @JsonProperty("ty") + public double ty; - /** - * Gets a string from the Limelight NetworkTable. - */ - public static String getLimelightNTString(String tableName, String entryName) { - return getLimelightNTTableEntry(tableName, entryName).getString(""); - } + @JsonProperty("txp") + public double tx_pixels; - /** - * Sets a double value in the Limelight NetworkTable. - */ - public static void setLimelightNTDouble(String tableName, String entryName, double val) { - getLimelightNTTableEntry(tableName, entryName).setDouble(val); - } + @JsonProperty("typ") + public double ty_pixels; - /** - * Sets a double array in the Limelight NetworkTable. - */ - public static void setLimelightNTDoubleArray(String tableName, String entryName, double[] vals) { - getLimelightNTTableEntry(tableName, entryName).setDoubleArray(vals); - } + @JsonProperty("tx_nocross") + public double tx_nocrosshair; - // ========== BASIC TARGETING DATA ========== + @JsonProperty("ty_nocross") + public double ty_nocrosshair; - /** - * Gets the horizontal offset from crosshair to target (degrees). - */ - public static double getTX(String limelightName) { - return getLimelightNTDouble(limelightName, "tx"); - } + @JsonProperty("ts") + public double ts; + + public LimelightTarget_Retro() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } - /** - * Gets whether the Limelight has a valid target (0 or 1). - */ - public static double getTV(String limelightName) { - return getLimelightNTDouble(limelightName, "tv"); } /** - * Gets the vertical offset from crosshair to target (degrees). + * Represents an AprilTag/Fiducial Target Result extracted from JSON Output */ - public static double getTY(String limelightName) { - return getLimelightNTDouble(limelightName, "ty"); - } + public static class LimelightTarget_Fiducial { - /** - * Gets the target area (0% to 100% of image). - */ - public static double getTA(String limelightName) { - return getLimelightNTDouble(limelightName, "ta"); - } + @JsonProperty("fID") + public double fiducialID; - /** - * Gets the pipeline's latency contribution (ms). Add to "cl" for total latency. - */ - public static double getLatency_Pipeline(String limelightName) { - return getLimelightNTDouble(limelightName, "tl"); - } + @JsonProperty("fam") + public String fiducialFamily; - /** - * Gets the capture pipeline latency (ms). - */ - public static double getLatency_Capture(String limelightName) { - return getLimelightNTDouble(limelightName, "cl"); - } + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; - /** - * Gets the JSON dump of targeting results. - */ - public static String getJSONDump(String limelightName) { - return getLimelightNTString(limelightName, "json"); - } + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; - // ========== POSE ESTIMATION ========== + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; - /** - * Converts a double array to Pose3d. - */ - public static Pose3d toPose3D(double[] inData) { - if (inData.length < 6) { - return new Pose3d(); + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() + { + return toPose3D(cameraPose_TargetSpace); + } + public Pose3d getRobotPose_FieldSpace() + { + return toPose3D(robotPose_FieldSpace); + } + public Pose3d getRobotPose_TargetSpace() + { + return toPose3D(robotPose_TargetSpace); + } + public Pose3d getTargetPose_CameraSpace() + { + return toPose3D(targetPose_CameraSpace); + } + public Pose3d getTargetPose_RobotSpace() + { + return toPose3D(targetPose_RobotSpace); } - return new Pose3d( - new Translation3d(inData[0], inData[1], inData[2]), - new Rotation3d( - Math.toRadians(inData[3]), - Math.toRadians(inData[4]), - Math.toRadians(inData[5]) - ) - ); - } - /** - * Converts a double array to Pose2d. - */ - public static Pose2d toPose2D(double[] inData) { - if (inData.length < 6) { - return new Pose2d(); + public Pose2d getCameraPose_TargetSpace2D() + { + return toPose2D(cameraPose_TargetSpace); + } + public Pose2d getRobotPose_FieldSpace2D() + { + return toPose2D(robotPose_FieldSpace); + } + public Pose2d getRobotPose_TargetSpace2D() + { + return toPose2D(robotPose_TargetSpace); + } + public Pose2d getTargetPose_CameraSpace2D() + { + return toPose2D(targetPose_CameraSpace); + } + public Pose2d getTargetPose_RobotSpace2D() + { + return toPose2D(targetPose_RobotSpace); } - return new Pose2d( - new Translation2d(inData[0], inData[1]), - Rotation2d.fromDegrees(inData[5]) - ); - } - /** - * Gets the robot's position in field space (meters). - */ - public static double[] getBotpose(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose"); - } + @JsonProperty("ta") + public double ta; - /** - * Gets the robot's position in field space (blue alliance origin). - */ - public static double[] getBotpose_wpiBlue(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); - } + @JsonProperty("tx") + public double tx; - /** - * Gets the robot's position in field space (red alliance origin). - */ - public static double[] getBotpose_wpiRed(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); - } + @JsonProperty("ty") + public double ty; - /** - * Gets the robot's position relative to the primary AprilTag. - */ - public static double[] getBotpose_TargetSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); - } + @JsonProperty("txp") + public double tx_pixels; - /** - * Gets the camera's position relative to the primary AprilTag. - */ - public static double[] getCameraPose_TargetSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); - } + @JsonProperty("typ") + public double ty_pixels; - /** - * Gets the camera's position relative to the robot. - */ - public static double[] getCameraPose_RobotSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "camerapose_robotspace"); - } + @JsonProperty("tx_nocross") + public double tx_nocrosshair; - /** - * Gets the target's position relative to the camera. - */ - public static double[] getTargetPose_CameraSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); - } + @JsonProperty("ty_nocross") + public double ty_nocrosshair; - /** - * Gets the target's position relative to the robot. - */ - public static double[] getTargetPose_RobotSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + @JsonProperty("ts") + public double ts; + + public LimelightTarget_Fiducial() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } } /** - * Gets the average color of the target (HSV). + * Represents a Barcode Target Result extracted from JSON Output */ - public static double[] getTargetColor(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "tc"); - } + public static class LimelightTarget_Barcode { - /** - * Gets the ID of the primary AprilTag. - */ - public static double getFiducialID(String limelightName) { - return getLimelightNTDouble(limelightName, "tid"); - } + /** + * Barcode family type (e.g. "QR", "DataMatrix", etc.) + */ + @JsonProperty("fam") + public String family; - /** - * Gets the class ID of the detected object. - */ - public static String getNeuralClassID(String limelightName) { - return getLimelightNTString(limelightName, "tclass"); - } + /** + * Gets the decoded data content of the barcode + */ + @JsonProperty("data") + public String data; - // ========== CONFIGURATION ========== + @JsonProperty("txp") + public double tx_pixels; - /** - * Sets the active pipeline index (0-9). - */ - public static void setPipelineIndex(String limelightName, int index) { - setLimelightNTDouble(limelightName, "pipeline", index); - } + @JsonProperty("typ") + public double ty_pixels; - /** - * Sets the priority tag ID for multi-tag detection. - */ - public static void setPriorityTagID(String limelightName, int ID) { - setLimelightNTDouble(limelightName, "priorityid", ID); + @JsonProperty("tx") + public double tx; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + + @JsonProperty("ta") + public double ta; + + @JsonProperty("pts") + public double[][] corners; + + public LimelightTarget_Barcode() { + } + + public String getFamily() { + return family; + } } /** - * Sets LED mode to pipeline control. + * Represents a Neural Classifier Pipeline Result extracted from JSON Output */ - public static void setLEDMode_PipelineControl(String limelightName) { - setLimelightNTDouble(limelightName, "ledMode", 0); + public static class LimelightTarget_Classifier { + + @JsonProperty("class") + public String className; + + @JsonProperty("classID") + public double classID; + + @JsonProperty("conf") + public double confidence; + + @JsonProperty("zone") + public double zone; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + public LimelightTarget_Classifier() { + } } /** - * Forces LEDs off. + * Represents a Neural Detector Pipeline Result extracted from JSON Output */ - public static void setLEDMode_ForceOff(String limelightName) { - setLimelightNTDouble(limelightName, "ledMode", 1); + public static class LimelightTarget_Detector { + + @JsonProperty("class") + public String className; + + @JsonProperty("classID") + public double classID; + + @JsonProperty("conf") + public double confidence; + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + + public LimelightTarget_Detector() { + } } /** - * Forces LEDs to blink. + * Limelight Results object, parsed from a Limelight's JSON results output. */ - public static void setLEDMode_ForceBlink(String limelightName) { - setLimelightNTDouble(limelightName, "ledMode", 2); + public static class LimelightResults { + + public String error; + + @JsonProperty("pID") + public double pipelineID; + + @JsonProperty("tl") + public double latency_pipeline; + + @JsonProperty("cl") + public double latency_capture; + + public double latency_jsonParse; + + @JsonProperty("ts") + public double timestamp_LIMELIGHT_publish; + + @JsonProperty("ts_rio") + public double timestamp_RIOFPGA_capture; + + @JsonProperty("v") + @JsonFormat(shape = Shape.NUMBER) + public boolean valid; + + @JsonProperty("botpose") + public double[] botpose; + + @JsonProperty("botpose_wpired") + public double[] botpose_wpired; + + @JsonProperty("botpose_wpiblue") + public double[] botpose_wpiblue; + + @JsonProperty("botpose_tagcount") + public double botpose_tagcount; + + @JsonProperty("botpose_span") + public double botpose_span; + + @JsonProperty("botpose_avgdist") + public double botpose_avgdist; + + @JsonProperty("botpose_avgarea") + public double botpose_avgarea; + + @JsonProperty("t6c_rs") + public double[] camerapose_robotspace; + + public Pose3d getBotPose3d() { + return toPose3D(botpose); + } + + public Pose3d getBotPose3d_wpiRed() { + return toPose3D(botpose_wpired); + } + + public Pose3d getBotPose3d_wpiBlue() { + return toPose3D(botpose_wpiblue); + } + + public Pose2d getBotPose2d() { + return toPose2D(botpose); + } + + public Pose2d getBotPose2d_wpiRed() { + return toPose2D(botpose_wpired); + } + + public Pose2d getBotPose2d_wpiBlue() { + return toPose2D(botpose_wpiblue); + } + + @JsonProperty("Retro") + public LimelightTarget_Retro[] targets_Retro; + + @JsonProperty("Fiducial") + public LimelightTarget_Fiducial[] targets_Fiducials; + + @JsonProperty("Classifier") + public LimelightTarget_Classifier[] targets_Classifier; + + @JsonProperty("Detector") + public LimelightTarget_Detector[] targets_Detector; + + @JsonProperty("Barcode") + public LimelightTarget_Barcode[] targets_Barcode; + + public LimelightResults() { + botpose = new double[6]; + botpose_wpired = new double[6]; + botpose_wpiblue = new double[6]; + camerapose_robotspace = new double[6]; + targets_Retro = new LimelightTarget_Retro[0]; + targets_Fiducials = new LimelightTarget_Fiducial[0]; + targets_Classifier = new LimelightTarget_Classifier[0]; + targets_Detector = new LimelightTarget_Detector[0]; + targets_Barcode = new LimelightTarget_Barcode[0]; + + } + + } /** - * Forces LEDs on. + * Represents a Limelight Raw Fiducial result from Limelight's NetworkTables output. */ - public static void setLEDMode_ForceOn(String limelightName) { - setLimelightNTDouble(limelightName, "ledMode", 3); + public static class RawFiducial { + public int id = 0; + public double txnc = 0; + public double tync = 0; + public double ta = 0; + public double distToCamera = 0; + public double distToRobot = 0; + public double ambiguity = 0; + + + public RawFiducial(int id, double txnc, double tync, double ta, double distToCamera, double distToRobot, double ambiguity) { + this.id = id; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.distToCamera = distToCamera; + this.distToRobot = distToRobot; + this.ambiguity = ambiguity; + } } /** - * Sets stream mode to standard (side-by-side if two cameras). + * Represents a Limelight Raw Neural Detector result from Limelight's NetworkTables output. */ - public static void setStreamMode_Standard(String limelightName) { - setLimelightNTDouble(limelightName, "stream", 0); + public static class RawDetection { + public int classId = 0; + public double txnc = 0; + public double tync = 0; + public double ta = 0; + public double corner0_X = 0; + public double corner0_Y = 0; + public double corner1_X = 0; + public double corner1_Y = 0; + public double corner2_X = 0; + public double corner2_Y = 0; + public double corner3_X = 0; + public double corner3_Y = 0; + + + public RawDetection(int classId, double txnc, double tync, double ta, + double corner0_X, double corner0_Y, + double corner1_X, double corner1_Y, + double corner2_X, double corner2_Y, + double corner3_X, double corner3_Y ) { + this.classId = classId; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.corner0_X = corner0_X; + this.corner0_Y = corner0_Y; + this.corner1_X = corner1_X; + this.corner1_Y = corner1_Y; + this.corner2_X = corner2_X; + this.corner2_Y = corner2_Y; + this.corner3_X = corner3_X; + this.corner3_Y = corner3_Y; + } } /** - * Sets stream mode to PiP Main (secondary camera in primary). + * Represents a 3D Pose Estimate. */ - public static void setStreamMode_PiPMain(String limelightName) { - setLimelightNTDouble(limelightName, "stream", 1); - } + public static class PoseEstimate { + public Pose2d pose; + public double timestampSeconds; + public double latency; + public int tagCount; + public double tagSpan; + public double avgTagDist; + public double avgTagArea; - /** - * Sets stream mode to PiP Secondary (primary camera in secondary). - */ - public static void setStreamMode_PiPSecondary(String limelightName) { - setLimelightNTDouble(limelightName, "stream", 2); - } + public RawFiducial[] rawFiducials; + public boolean isMegaTag2; - /** - * Sets the crop window for the pipeline. - * The crop window in the UI must be completely open for dynamic cropping to work. - */ - public static void setCropWindow(String limelightName, double cropXMin, double cropXMax, - double cropYMin, double cropYMax) { - double[] cropWindow = {cropXMin, cropXMax, cropYMin, cropYMax}; - setLimelightNTDoubleArray(limelightName, "crop", cropWindow); - } + /** + * Instantiates a PoseEstimate object with default values + */ + public PoseEstimate() { + this.pose = new Pose2d(); + this.timestampSeconds = 0; + this.latency = 0; + this.tagCount = 0; + this.tagSpan = 0; + this.avgTagDist = 0; + this.avgTagArea = 0; + this.rawFiducials = new RawFiducial[]{}; + this.isMegaTag2 = false; + } - /** - * Sets the robot's orientation for MegaTag2. - * - * @param yaw Yaw in degrees - * @param yawRate Yaw rate in degrees/sec - * @param pitch Pitch in degrees - * @param pitchRate Pitch rate in degrees/sec - * @param roll Roll in degrees - * @param rollRate Roll rate in degrees/sec - */ - public static void setRobotOrientation(String limelightName, double yaw, double yawRate, - double pitch, double pitchRate, double roll, double rollRate) { - double[] entries = {yaw, yawRate, pitch, pitchRate, roll, rollRate}; - setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries); - } + public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, + int tagCount, double tagSpan, double avgTagDist, + double avgTagArea, RawFiducial[] rawFiducials, boolean isMegaTag2) { + + this.pose = pose; + this.timestampSeconds = timestampSeconds; + this.latency = latency; + this.tagCount = tagCount; + this.tagSpan = tagSpan; + this.avgTagDist = avgTagDist; + this.avgTagArea = avgTagArea; + this.rawFiducials = rawFiducials; + this.isMegaTag2 = isMegaTag2; + } - /** - * Sets fiducial downscaling for performance. - * - * @param downscale Valid values: 1.0, 1.5, 2.0, 3.0, 4.0 - */ - public static void setFiducialDownscaling(String limelightName, double downscale) { - int d = 1; // Default to 1.0 - if (downscale == 1.5) d = 2; - else if (downscale == 2.0) d = 3; - else if (downscale == 3.0) d = 4; - else if (downscale == 4.0) d = 5; - setLimelightNTDouble(limelightName, "fiducial_downscale_set", d); } /** - * Sets which AprilTag IDs the pipeline should look for. - */ - public static void setFiducialIDFiltersOverride(String limelightName, int[] validIDs) { - double[] validIDsDouble = new double[validIDs.length]; - for (int i = 0; i < validIDs.length; i++) { - validIDsDouble[i] = validIDs[i]; + * Encapsulates the state of an internal Limelight IMU. + */ + public static class IMUData { + public double robotYaw = 0.0; + public double Roll = 0.0; + public double Pitch = 0.0; + public double Yaw = 0.0; + public double gyroX = 0.0; + public double gyroY = 0.0; + public double gyroZ = 0.0; + public double accelX = 0.0; + public double accelY = 0.0; + public double accelZ = 0.0; + + public IMUData() {} + + public IMUData(double[] imuData) { + if (imuData != null && imuData.length >= 10) { + this.robotYaw = imuData[0]; + this.Roll = imuData[1]; + this.Pitch = imuData[2]; + this.Yaw = imuData[3]; + this.gyroX = imuData[4]; + this.gyroY = imuData[5]; + this.gyroZ = imuData[6]; + this.accelX = imuData[7]; + this.accelY = imuData[8]; + this.accelZ = imuData[9]; + } } - setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble); } + + private static ObjectMapper mapper; + /** - * Sets the camera pose in robot space. - * The UI camera pose must be set to zeros for this to work. + * Print JSON Parse time to the console in milliseconds */ - public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, - double up, double roll, double pitch, double yaw) { - double[] entries = {forward, side, up, roll, pitch, yaw}; - setLimelightNTDoubleArray(limelightName, "camerapose_robotspace_set", entries); + static boolean profileJSON = false; + + static final String sanitizeName(String name) { + if (name == "" || name == null) { + return "limelight"; + } + return name; } /** - * Sets data to be sent to Python scripts. + * Takes a 6-length array of pose data and converts it to a Pose3d object. + * Array format: [x, y, z, roll, pitch, yaw] where angles are in degrees. + * @param inData Array containing pose data [x, y, z, roll, pitch, yaw] + * @return Pose3d object representing the pose, or empty Pose3d if invalid data */ - public static void setPythonScriptData(String limelightName, double[] outgoingPythonData) { - setLimelightNTDoubleArray(limelightName, "llrobot", outgoingPythonData); + public static Pose3d toPose3D(double[] inData){ + if(inData.length < 6) + { + //System.err.println("Bad LL 3D Pose Data!"); + return new Pose3d(); + } + return new Pose3d( + new Translation3d(inData[0], inData[1], inData[2]), + new Rotation3d(Units.degreesToRadians(inData[3]), Units.degreesToRadians(inData[4]), + Units.degreesToRadians(inData[5]))); } /** - * Gets data from Python scripts. + * Takes a 6-length array of pose data and converts it to a Pose2d object. + * Uses only x, y, and yaw components, ignoring z, roll, and pitch. + * Array format: [x, y, z, roll, pitch, yaw] where angles are in degrees. + * @param inData Array containing pose data [x, y, z, roll, pitch, yaw] + * @return Pose2d object representing the pose, or empty Pose2d if invalid data */ - public static double[] getPythonScriptData(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "llpython"); + public static Pose2d toPose2D(double[] inData){ + if(inData.length < 6) + { + //System.err.println("Bad LL 2D Pose Data!"); + return new Pose2d(); + } + Translation2d tran2d = new Translation2d(inData[0], inData[1]); + Rotation2d r2d = new Rotation2d(Units.degreesToRadians(inData[5])); + return new Pose2d(tran2d, r2d); + } + + /** + * Converts a Pose3d object to an array of doubles in the format [x, y, z, roll, pitch, yaw]. + * Translation components are in meters, rotation components are in degrees. + * + * @param pose The Pose3d object to convert + * @return A 6-element array containing [x, y, z, roll, pitch, yaw] + */ + public static double[] pose3dToArray(Pose3d pose) { + double[] result = new double[6]; + result[0] = pose.getTranslation().getX(); + result[1] = pose.getTranslation().getY(); + result[2] = pose.getTranslation().getZ(); + result[3] = Units.radiansToDegrees(pose.getRotation().getX()); + result[4] = Units.radiansToDegrees(pose.getRotation().getY()); + result[5] = Units.radiansToDegrees(pose.getRotation().getZ()); + return result; + } + + /** + * Converts a Pose2d object to an array of doubles in the format [x, y, z, roll, pitch, yaw]. + * Translation components are in meters, rotation components are in degrees. + * Note: z, roll, and pitch will be 0 since Pose2d only contains x, y, and yaw. + * + * @param pose The Pose2d object to convert + * @return A 6-element array containing [x, y, 0, 0, 0, yaw] + */ + public static double[] pose2dToArray(Pose2d pose) { + double[] result = new double[6]; + result[0] = pose.getTranslation().getX(); + result[1] = pose.getTranslation().getY(); + result[2] = 0; + result[3] = Units.radiansToDegrees(0); + result[4] = Units.radiansToDegrees(0); + result[5] = Units.radiansToDegrees(pose.getRotation().getRadians()); + return result; + } + + private static double extractArrayEntry(double[] inData, int position){ + if(inData.length < position+1) + { + return 0; + } + return inData[position]; } - // ========== HELPER METHODS ========== + private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName, boolean isMegaTag2) { + DoubleArrayEntry poseEntry = LimelightHelpers.getLimelightDoubleArrayEntry(limelightName, entryName); + + TimestampedDoubleArray tsValue = poseEntry.getAtomic(); + double[] poseArray = tsValue.value; + long timestamp = tsValue.timestamp; - private static double extractArrayEntry(double[] inData, int position) { - if (inData.length < position + 1) { - return 0.0; + if (poseArray.length == 0) { + // Handle the case where no data is available + return null; // or some default PoseEstimate } - return inData[position]; - } - // ========== RAW FIDUCIALS ========== + var pose = toPose2D(poseArray); + double latency = extractArrayEntry(poseArray, 6); + int tagCount = (int)extractArrayEntry(poseArray, 7); + double tagSpan = extractArrayEntry(poseArray, 8); + double tagDist = extractArrayEntry(poseArray, 9); + double tagArea = extractArrayEntry(poseArray, 10); - /** - * Represents a raw fiducial detection. - */ - public static class RawFiducial { - public int id; - public double txnc; - public double tync; - public double ta; - public double distToCamera; - public double distToRobot; - public double ambiguity; + // Convert server timestamp from microseconds to seconds and adjust for latency + double adjustedTimestamp = (timestamp / 1000000.0) - (latency / 1000.0); - public RawFiducial(int id, double txnc, double tync, double ta, - double distToCamera, double distToRobot, double ambiguity) { - this.id = id; - this.txnc = txnc; - this.tync = tync; - this.ta = ta; - this.distToCamera = distToCamera; - this.distToRobot = distToRobot; - this.ambiguity = ambiguity; + RawFiducial[] rawFiducials = new RawFiducial[tagCount]; + int valsPerFiducial = 7; + int expectedTotalVals = 11 + valsPerFiducial * tagCount; + + if (poseArray.length != expectedTotalVals) { + // Don't populate fiducials + } else { + for(int i = 0; i < tagCount; i++) { + int baseIndex = 11 + (i * valsPerFiducial); + int id = (int)poseArray[baseIndex]; + double txnc = poseArray[baseIndex + 1]; + double tync = poseArray[baseIndex + 2]; + double ta = poseArray[baseIndex + 3]; + double distToCamera = poseArray[baseIndex + 4]; + double distToRobot = poseArray[baseIndex + 5]; + double ambiguity = poseArray[baseIndex + 6]; + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } } + + return new PoseEstimate(pose, adjustedTimestamp, latency, tagCount, tagSpan, tagDist, tagArea, rawFiducials, isMegaTag2); } /** - * Gets raw fiducial data from the Limelight. + * Gets the latest raw fiducial/AprilTag detection results from NetworkTables. + * + * @param limelightName Name/identifier of the Limelight + * @return Array of RawFiducial objects containing detection details */ - public static List getRawFiducials(String limelightName) { - double[] rawFiducialArray = getLimelightNTDoubleArray(limelightName, "rawfiducials"); + public static RawFiducial[] getRawFiducials(String limelightName) { + var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawfiducials"); + var rawFiducialArray = entry.getDoubleArray(new double[0]); int valsPerEntry = 7; - if (rawFiducialArray.length % valsPerEntry != 0) { - return new ArrayList<>(); + return new RawFiducial[0]; } int numFiducials = rawFiducialArray.length / valsPerEntry; - List rawFiducials = new ArrayList<>(); + RawFiducial[] rawFiducials = new RawFiducial[numFiducials]; for (int i = 0; i < numFiducials; i++) { int baseIndex = i * valsPerEntry; @@ -448,65 +775,31 @@ public static List getRawFiducials(String limelightName) { double distToRobot = extractArrayEntry(rawFiducialArray, baseIndex + 5); double ambiguity = extractArrayEntry(rawFiducialArray, baseIndex + 6); - rawFiducials.add(new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity)); + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); } return rawFiducials; } - // ========== RAW DETECTIONS ========== - - /** - * Represents a raw neural network detection. - */ - public static class RawDetection { - public int classId; - public double txnc; - public double tync; - public double ta; - public double corner0_X; - public double corner0_Y; - public double corner1_X; - public double corner1_Y; - public double corner2_X; - public double corner2_Y; - public double corner3_X; - public double corner3_Y; - - public RawDetection(int classId, double txnc, double tync, double ta, - double corner0_X, double corner0_Y, double corner1_X, double corner1_Y, - double corner2_X, double corner2_Y, double corner3_X, double corner3_Y) { - this.classId = classId; - this.txnc = txnc; - this.tync = tync; - this.ta = ta; - this.corner0_X = corner0_X; - this.corner0_Y = corner0_Y; - this.corner1_X = corner1_X; - this.corner1_Y = corner1_Y; - this.corner2_X = corner2_X; - this.corner2_Y = corner2_Y; - this.corner3_X = corner3_X; - this.corner3_Y = corner3_Y; - } - } - /** - * Gets raw detection data from the Limelight. + * Gets the latest raw neural detector results from NetworkTables + * + * @param limelightName Name/identifier of the Limelight + * @return Array of RawDetection objects containing detection details */ - public static List getRawDetections(String limelightName) { - double[] rawDetectionArray = getLimelightNTDoubleArray(limelightName, "rawdetections"); + public static RawDetection[] getRawDetections(String limelightName) { + var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawdetections"); + var rawDetectionArray = entry.getDoubleArray(new double[0]); int valsPerEntry = 12; - if (rawDetectionArray.length % valsPerEntry != 0) { - return new ArrayList<>(); + return new RawDetection[0]; } int numDetections = rawDetectionArray.length / valsPerEntry; - List rawDetections = new ArrayList<>(); + RawDetection[] rawDetections = new RawDetection[numDetections]; for (int i = 0; i < numDetections; i++) { - int baseIndex = i * valsPerEntry; + int baseIndex = i * valsPerEntry; // Starting index for this detection's data int classId = (int) extractArrayEntry(rawDetectionArray, baseIndex); double txnc = extractArrayEntry(rawDetectionArray, baseIndex + 1); double tync = extractArrayEntry(rawDetectionArray, baseIndex + 2); @@ -520,565 +813,835 @@ public static List getRawDetections(String limelightName) { double corner3_X = extractArrayEntry(rawDetectionArray, baseIndex + 10); double corner3_Y = extractArrayEntry(rawDetectionArray, baseIndex + 11); - rawDetections.add(new RawDetection(classId, txnc, tync, ta, - corner0_X, corner0_Y, corner1_X, corner1_Y, - corner2_X, corner2_Y, corner3_X, corner3_Y)); + rawDetections[i] = new RawDetection(classId, txnc, tync, ta, corner0_X, corner0_Y, corner1_X, corner1_Y, corner2_X, corner2_Y, corner3_X, corner3_Y); } return rawDetections; } - // ========== POSE ESTIMATE ========== + /** + * Prints detailed information about a PoseEstimate to standard output. + * Includes timestamp, latency, tag count, tag span, average tag distance, + * average tag area, and detailed information about each detected fiducial. + * + * @param pose The PoseEstimate object to print. If null, prints "No PoseEstimate available." + */ + public static void printPoseEstimate(PoseEstimate pose) { + if (pose == null) { + System.out.println("No PoseEstimate available."); + return; + } + + System.out.printf("Pose Estimate Information:%n"); + System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds); + System.out.printf("Latency: %.3f ms%n", pose.latency); + System.out.printf("Tag Count: %d%n", pose.tagCount); + System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan); + System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist); + System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea); + System.out.printf("Is MegaTag2: %b%n", pose.isMegaTag2); + System.out.println(); + + if (pose.rawFiducials == null || pose.rawFiducials.length == 0) { + System.out.println("No RawFiducials data available."); + return; + } + + System.out.println("Raw Fiducials Details:"); + for (int i = 0; i < pose.rawFiducials.length; i++) { + RawFiducial fiducial = pose.rawFiducials[i]; + System.out.printf(" Fiducial #%d:%n", i + 1); + System.out.printf(" ID: %d%n", fiducial.id); + System.out.printf(" TXNC: %.2f%n", fiducial.txnc); + System.out.printf(" TYNC: %.2f%n", fiducial.tync); + System.out.printf(" TA: %.2f%n", fiducial.ta); + System.out.printf(" Distance to Camera: %.2f meters%n", fiducial.distToCamera); + System.out.printf(" Distance to Robot: %.2f meters%n", fiducial.distToRobot); + System.out.printf(" Ambiguity: %.2f%n", fiducial.ambiguity); + System.out.println(); + } + } + + public static Boolean validPoseEstimate(PoseEstimate pose) { + return pose != null && pose.rawFiducials != null && pose.rawFiducials.length != 0; + } + + public static NetworkTable getLimelightNTTable(String tableName) { + return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); + } + + public static void Flush() { + NetworkTableInstance.getDefault().flush(); + } + + public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) { + return getLimelightNTTable(tableName).getEntry(entryName); + } + + public static DoubleArrayEntry getLimelightDoubleArrayEntry(String tableName, String entryName) { + String key = tableName + "/" + entryName; + return doubleArrayEntries.computeIfAbsent(key, k -> { + NetworkTable table = getLimelightNTTable(tableName); + return table.getDoubleArrayTopic(entryName).getEntry(new double[0]); + }); + } + + public static double getLimelightNTDouble(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0); + } + + public static void setLimelightNTDouble(String tableName, String entryName, double val) { + getLimelightNTTableEntry(tableName, entryName).setDouble(val); + } + + public static void setLimelightNTDoubleArray(String tableName, String entryName, double[] val) { + getLimelightNTTableEntry(tableName, entryName).setDoubleArray(val); + } + + public static double[] getLimelightNTDoubleArray(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]); + } + + + public static String getLimelightNTString(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getString(""); + } + + public static String[] getLimelightNTStringArray(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getStringArray(new String[0]); + } + + + public static URL getLimelightURLString(String tableName, String request) { + String urlString = "http://" + sanitizeName(tableName) + ".local:5807/" + request; + URL url; + try { + url = new URL(urlString); + return url; + } catch (MalformedURLException e) { + System.err.println("bad LL URL"); + } + return null; + } + ///// + ///// /** - * Represents a complete pose estimate from the Limelight. + * Does the Limelight have a valid target? + * @param limelightName Name of the Limelight camera ("" for default) + * @return True if a valid target is present, false otherwise */ - public static class PoseEstimate { - public Pose2d pose; - public double timestampSeconds; - public double latency; - public int tagCount; - public double tagSpan; - public double avgTagDist; - public double avgTagArea; - public List rawFiducials; + public static boolean getTV(String limelightName) { + return 1.0 == getLimelightNTDouble(limelightName, "tv"); + } - public PoseEstimate() { - this.pose = new Pose2d(); - this.timestampSeconds = 0.0; - this.latency = 0.0; - this.tagCount = 0; - this.tagSpan = 0.0; - this.avgTagDist = 0.0; - this.avgTagArea = 0.0; - this.rawFiducials = new ArrayList<>(); + /** + * Gets the horizontal offset from the crosshair to the target in degrees. + * @param limelightName Name of the Limelight camera ("" for default) + * @return Horizontal offset angle in degrees + */ + public static double getTX(String limelightName) { + return getLimelightNTDouble(limelightName, "tx"); + } + + /** + * Gets the vertical offset from the crosshair to the target in degrees. + * @param limelightName Name of the Limelight camera ("" for default) + * @return Vertical offset angle in degrees + */ + public static double getTY(String limelightName) { + return getLimelightNTDouble(limelightName, "ty"); + } + + /** + * Gets the horizontal offset from the principal pixel/point to the target in degrees. This is the most accurate 2d metric if you are using a calibrated camera and you don't need adjustable crosshair functionality. + * @param limelightName Name of the Limelight camera ("" for default) + * @return Horizontal offset angle in degrees + */ + public static double getTXNC(String limelightName) { + return getLimelightNTDouble(limelightName, "txnc"); + } + + /** + * Gets the vertical offset from the principal pixel/point to the target in degrees. This is the most accurate 2d metric if you are using a calibrated camera and you don't need adjustable crosshair functionality. + * @param limelightName Name of the Limelight camera ("" for default) + * @return Vertical offset angle in degrees + */ + public static double getTYNC(String limelightName) { + return getLimelightNTDouble(limelightName, "tync"); + } + + /** + * Gets the target area as a percentage of the image (0-100%). + * @param limelightName Name of the Limelight camera ("" for default) + * @return Target area percentage (0-100) + */ + public static double getTA(String limelightName) { + return getLimelightNTDouble(limelightName, "ta"); + } + + /** + * T2D is an array that contains several targeting metrcis + * @param limelightName Name of the Limelight camera + * @return Array containing [targetValid, targetCount, targetLatency, captureLatency, tx, ty, txnc, tync, ta, tid, targetClassIndexDetector, + * targetClassIndexClassifier, targetLongSidePixels, targetShortSidePixels, targetHorizontalExtentPixels, targetVerticalExtentPixels, targetSkewDegrees] + */ + public static double[] getT2DArray(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "t2d"); + } + + /** + * Gets the number of targets currently detected. + * @param limelightName Name of the Limelight camera + * @return Number of detected targets + */ + public static int getTargetCount(String limelightName) { + double[] t2d = getT2DArray(limelightName); + if(t2d.length == 17) + { + return (int)t2d[1]; } + return 0; + } - public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, - int tagCount, double tagSpan, double avgTagDist, double avgTagArea, - List rawFiducials) { - this.pose = pose; - this.timestampSeconds = timestampSeconds; - this.latency = latency; - this.tagCount = tagCount; - this.tagSpan = tagSpan; - this.avgTagDist = avgTagDist; - this.avgTagArea = avgTagArea; - this.rawFiducials = rawFiducials; + /** + * Gets the classifier class index from the currently running neural classifier pipeline + * @param limelightName Name of the Limelight camera + * @return Class index from classifier pipeline + */ + public static int getClassifierClassIndex (String limelightName) { + double[] t2d = getT2DArray(limelightName); + if(t2d.length == 17) + { + return (int)t2d[10]; } + return 0; } /** - * Gets a pose estimate from the specified NetworkTable entry. + * Gets the detector class index from the primary result of the currently running neural detector pipeline. + * @param limelightName Name of the Limelight camera + * @return Class index from detector pipeline */ - private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName) { - NetworkTableEntry poseEntry = getLimelightNTTableEntry(limelightName, entryName); - double[] poseArray = poseEntry.getDoubleArray(new double[0]); - - if (poseArray.length == 0) { - return null; + public static int getDetectorClassIndex (String limelightName) { + double[] t2d = getT2DArray(limelightName); + if(t2d.length == 17) + { + return (int)t2d[11]; } + return 0; + } - Pose2d pose = toPose2D(poseArray); - double latency = extractArrayEntry(poseArray, 6); - int tagCount = (int) extractArrayEntry(poseArray, 7); - double tagSpan = extractArrayEntry(poseArray, 8); - double tagDist = extractArrayEntry(poseArray, 9); - double tagArea = extractArrayEntry(poseArray, 10); + /** + * Gets the current neural classifier result class name. + * @param limelightName Name of the Limelight camera + * @return Class name string from classifier pipeline + */ + public static String getClassifierClass (String limelightName) { + return getLimelightNTString(limelightName, "tcclass"); + } - // Calculate timestamp: getLastChange is in microseconds, latency is in milliseconds - double timestamp = (poseEntry.getLastChange() / 1000000.0) - (latency / 1000.0); + /** + * Gets the primary neural detector result class name. + * @param limelightName Name of the Limelight camera + * @return Class name string from detector pipeline + */ + public static String getDetectorClass (String limelightName) { + return getLimelightNTString(limelightName, "tdclass"); + } - List rawFiducials = new ArrayList<>(); - int valsPerFiducial = 7; - int expectedTotalVals = 11 + valsPerFiducial * tagCount; + /** + * Gets the pipeline's processing latency contribution. + * @param limelightName Name of the Limelight camera + * @return Pipeline latency in milliseconds + */ + public static double getLatency_Pipeline(String limelightName) { + return getLimelightNTDouble(limelightName, "tl"); + } - if (poseArray.length == expectedTotalVals) { - for (int i = 0; i < tagCount; i++) { - int baseIndex = 11 + (i * valsPerFiducial); - int id = (int) extractArrayEntry(poseArray, baseIndex); - double txnc = extractArrayEntry(poseArray, baseIndex + 1); - double tync = extractArrayEntry(poseArray, baseIndex + 2); - double ta = extractArrayEntry(poseArray, baseIndex + 3); - double distToCamera = extractArrayEntry(poseArray, baseIndex + 4); - double distToRobot = extractArrayEntry(poseArray, baseIndex + 5); - double ambiguity = extractArrayEntry(poseArray, baseIndex + 6); - rawFiducials.add(new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity)); - } - } + /** + * Gets the capture latency. + * @param limelightName Name of the Limelight camera + * @return Capture latency in milliseconds + */ + public static double getLatency_Capture(String limelightName) { + return getLimelightNTDouble(limelightName, "cl"); + } - return new PoseEstimate(pose, timestamp, latency, tagCount, tagSpan, tagDist, tagArea, rawFiducials); + /** + * Gets the active pipeline index. + * @param limelightName Name of the Limelight camera + * @return Current pipeline index (0-9) + */ + public static double getCurrentPipelineIndex(String limelightName) { + return getLimelightNTDouble(limelightName, "getpipe"); } /** - * Gets the robot pose estimate in WPILib blue alliance coordinates. + * Gets the current pipeline type. + * @param limelightName Name of the Limelight camera + * @return Pipeline type string (e.g. "retro", "apriltag", etc) */ - public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose_wpiblue"); + public static String getCurrentPipelineType(String limelightName) { + return getLimelightNTString(limelightName, "getpipetype"); } /** - * Gets the robot pose estimate in WPILib red alliance coordinates. + * Gets the full JSON results dump. + * @param limelightName Name of the Limelight camera + * @return JSON string containing all current results */ - public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose_wpired"); + public static String getJSONDump(String limelightName) { + return getLimelightNTString(limelightName, "json"); } /** - * Gets the robot pose estimate using MegaTag2 in blue alliance coordinates. + * Switch to getBotPose + * + * @param limelightName + * @return */ - public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue"); + @Deprecated + public static double[] getBotpose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); } /** - * Gets the robot pose estimate using MegaTag2 in red alliance coordinates. + * Switch to getBotPose_wpiRed + * + * @param limelightName + * @return */ - public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose_orb_wpired"); + @Deprecated + public static double[] getBotpose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); } - // ========== JSON RESULTS (Advanced Usage) ========== - // Note: This requires Jackson for JSON parsing - // Add to build.gradle: implementation 'com.fasterxml.jackson.core:jackson-databind:2.15.0' - - private static final ObjectMapper mapper = new ObjectMapper() - .configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false); - /** - * Results from JSON parsing - contains all targeting data. + * Switch to getBotPose_wpiBlue + * + * @param limelightName + * @return */ - public static class LimelightResults { - @JsonProperty("Results") - public Results targetingResults; - - public LimelightResults() { - this.targetingResults = new Results(); - } + @Deprecated + public static double[] getBotpose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); } - public static class Results { - @JsonProperty("pID") - public double pipelineID; + public static double[] getBotPose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } - @JsonProperty("tl") - public double latency_pipeline; + public static double[] getBotPose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } - @JsonProperty("cl") - public double latency_capture; + public static double[] getBotPose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } - @JsonProperty("ts") - public double timestamp_LIMELIGHT_publish; + public static double[] getBotPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + } - @JsonProperty("ts_rio") - public double timestamp_RIOFPGA_capture; + public static double[] getCameraPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + } - @JsonProperty("v") - public int valid; + public static double[] getTargetPose_CameraSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + } - @JsonProperty("botpose") - public double[] botpose; + public static double[] getTargetPose_RobotSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + } - @JsonProperty("botpose_wpiblue") - public double[] botpose_wpiblue; + public static double[] getTargetColor(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "tc"); + } - @JsonProperty("botpose_wpired") - public double[] botpose_wpired; + public static double getFiducialID(String limelightName) { + return getLimelightNTDouble(limelightName, "tid"); + } - @JsonProperty("Retro") - public List retroResults; + public static String getNeuralClassID(String limelightName) { + return getLimelightNTString(limelightName, "tclass"); + } - @JsonProperty("Fiducial") - public List fiducialResults; + public static String[] getRawBarcodeData(String limelightName) { + return getLimelightNTStringArray(limelightName, "rawbarcodes"); + } - @JsonProperty("Detector") - public List detectorResults; + ///// + ///// - @JsonProperty("Classifier") - public List classifierResults; + public static Pose3d getBotPose3d(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose"); + return toPose3D(poseArray); + } - public Results() { - this.retroResults = new ArrayList<>(); - this.fiducialResults = new ArrayList<>(); - this.detectorResults = new ArrayList<>(); - this.classifierResults = new ArrayList<>(); - } + /** + * (Not Recommended) Gets the robot's 3D pose in the WPILib Red Alliance Coordinate System. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the robot's position and orientation in Red Alliance field space + */ + public static Pose3d getBotPose3d_wpiRed(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + return toPose3D(poseArray); } - public static class RetroreflectiveResult { - @JsonProperty("tx") - public double tx; + /** + * (Recommended) Gets the robot's 3D pose in the WPILib Blue Alliance Coordinate System. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the robot's position and orientation in Blue Alliance field space + */ + public static Pose3d getBotPose3d_wpiBlue(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + return toPose3D(poseArray); + } - @JsonProperty("ty") - public double ty; + /** + * Gets the robot's 3D pose with respect to the currently tracked target's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the robot's position and orientation relative to the target + */ + public static Pose3d getBotPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + return toPose3D(poseArray); + } - @JsonProperty("ta") - public double ta; + /** + * Gets the camera's 3D pose with respect to the currently tracked target's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the camera's position and orientation relative to the target + */ + public static Pose3d getCameraPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + return toPose3D(poseArray); + } - @JsonProperty("pts") - public double[][] corners; + /** + * Gets the target's 3D pose with respect to the camera's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the target's position and orientation relative to the camera + */ + public static Pose3d getTargetPose3d_CameraSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + return toPose3D(poseArray); } - public static class FiducialResult { - @JsonProperty("fID") - public int fiducialID; + /** + * Gets the target's 3D pose with respect to the robot's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the target's position and orientation relative to the robot + */ + public static Pose3d getTargetPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + return toPose3D(poseArray); + } - @JsonProperty("fam") - public String family; + /** + * Gets the camera's 3D pose with respect to the robot's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the camera's position and orientation relative to the robot + */ + public static Pose3d getCameraPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_robotspace"); + return toPose3D(poseArray); + } - @JsonProperty("tx") - public double tx; + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiBlue(String limelightName) { - @JsonProperty("ty") - public double ty; + double[] result = getBotPose_wpiBlue(limelightName); + return toPose2D(result); + } - @JsonProperty("ta") - public double ta; + /** + * Gets the MegaTag1 Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) in the WPILib Blue alliance coordinate system. + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpiblue", false); + } - @JsonProperty("t6c_ts") - public double[] cameraPose_TargetSpace; + /** + * Gets the MegaTag2 Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) in the WPILib Blue alliance coordinate system. + * Make sure you are calling setRobotOrientation() before calling this method. + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue", true); + } - @JsonProperty("t6t_cs") - public double[] targetPose_CameraSpace; + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiRed(String limelightName) { - @JsonProperty("t6t_rs") - public double[] targetPose_RobotSpace; + double[] result = getBotPose_wpiRed(limelightName); + return toPose2D(result); - @JsonProperty("t6r_fs") - public double[] robotPose_FieldSpace; + } - @JsonProperty("pts") - public double[][] corners; + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED + * alliance + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpired", false); } - public static class DetectorResult { - @JsonProperty("class") - public String className; + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED + * alliance + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpired", true); + } - @JsonProperty("classID") - public int classID; + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d(String limelightName) { - @JsonProperty("conf") - public double confidence; + double[] result = getBotPose(limelightName); + return toPose2D(result); - @JsonProperty("tx") - public double tx; + } - @JsonProperty("ty") - public double ty; + /** + * Gets the current IMU data from NetworkTables. + * IMU data is formatted as [robotYaw, Roll, Pitch, Yaw, gyroX, gyroY, gyroZ, accelX, accelY, accelZ]. + * Returns all zeros if data is invalid or unavailable. + * + * @param limelightName Name/identifier of the Limelight + * @return IMUData object containing all current IMU data + */ + public static IMUData getIMUData(String limelightName) { + double[] imuData = getLimelightNTDoubleArray(limelightName, "imu"); + if (imuData == null || imuData.length < 10) { + return new IMUData(); // Returns object with all zeros + } + return new IMUData(imuData); + } - @JsonProperty("ta") - public double ta; + ///// + ///// - @JsonProperty("pts") - public double[][] corners; + public static void setPipelineIndex(String limelightName, int pipelineIndex) { + setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); } - public static class ClassifierResult { - @JsonProperty("class") - public String className; - @JsonProperty("classID") - public int classID; + public static void setPriorityTagID(String limelightName, int ID) { + setLimelightNTDouble(limelightName, "priorityid", ID); + } - @JsonProperty("conf") - public double confidence; + /** + * Sets LED mode to be controlled by the current pipeline. + * @param limelightName Name of the Limelight camera + */ + public static void setLEDMode_PipelineControl(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 0); + } - @JsonProperty("tx") - public double tx; + public static void setLEDMode_ForceOff(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 1); + } - @JsonProperty("ty") - public double ty; + public static void setLEDMode_ForceBlink(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 2); + } - @JsonProperty("ta") - public double ta; + public static void setLEDMode_ForceOn(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 3); } /** - * Gets the latest targeting results by parsing the JSON dump. - * This provides access to all detection data in a structured format. - * - * @param limelightName Name of the Limelight - * @return LimelightResults object containing all targeting data + * Enables standard side-by-side stream mode. + * @param limelightName Name of the Limelight camera */ - public static LimelightResults getLatestResults(String limelightName) { - long start = System.nanoTime(); - String jsonString = getJSONDump(limelightName); - - try { - LimelightResults results = mapper.readValue(jsonString, LimelightResults.class); - long end = System.nanoTime(); - double latencyMs = (end - start) / 1_000_000.0; - - // Store JSON parsing latency - if (results.targetingResults != null) { - // You can add a custom field to store this if needed - System.out.println("JSON parse latency: " + latencyMs + " ms"); - } - - return results; - } catch (JsonProcessingException e) { - System.err.println("Error parsing Limelight JSON: " + e.getMessage()); - return new LimelightResults(); - } + public static void setStreamMode_Standard(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 0); } /** - * Prints a summary of the Limelight results to the console. - * Useful for debugging. + * Enables Picture-in-Picture mode with secondary stream in the corner. + * @param limelightName Name of the Limelight camera */ - public static void printResults(LimelightResults results) { - if (results == null || results.targetingResults == null) { - System.out.println("No results available"); - return; - } - - Results r = results.targetingResults; - System.out.println("=== Limelight Results ==="); - System.out.println("Pipeline: " + r.pipelineID); - System.out.println("Valid: " + (r.valid == 1 ? "Yes" : "No")); - System.out.println("Latency: " + r.latency_pipeline + " ms"); - - if (r.botpose_wpiblue != null && r.botpose_wpiblue.length >= 6) { - Pose2d pose = toPose2D(r.botpose_wpiblue); - System.out.println("Robot Pose (Blue): " + pose.toString()); - } - - System.out.println("Fiducials detected: " + r.fiducialResults.size()); - for (FiducialResult fid : r.fiducialResults) { - System.out.println(" - ID: " + fid.fiducialID + ", TX: " + fid.tx + ", TY: " + fid.ty); - } - - System.out.println("Detections: " + r.detectorResults.size()); - for (DetectorResult det : r.detectorResults) { - System.out.println(" - Class: " + det.className + ", Confidence: " + det.confidence); - } + public static void setStreamMode_PiPMain(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 1); } - // ========== UTILITY METHODS FOR VISION PROCESSING ========== - /** - * Checks if the Limelight has any valid targets. + * Enables Picture-in-Picture mode with primary stream in the corner. + * @param limelightName Name of the Limelight camera */ - public static boolean hasTarget(String limelightName) { - return getTV(limelightName) == 1.0; + public static void setStreamMode_PiPSecondary(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 2); } + /** - * Gets the pipeline index currently running. + * Sets the crop window for the camera. The crop window in the UI must be completely open. + * @param limelightName Name of the Limelight camera + * @param cropXMin Minimum X value (-1 to 1) + * @param cropXMax Maximum X value (-1 to 1) + * @param cropYMin Minimum Y value (-1 to 1) + * @param cropYMax Maximum Y value (-1 to 1) */ - public static int getCurrentPipelineIndex(String limelightName) { - return (int) getLimelightNTDouble(limelightName, "getpipe"); + public static void setCropWindow(String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax) { + double[] entries = new double[4]; + entries[0] = cropXMin; + entries[1] = cropXMax; + entries[2] = cropYMin; + entries[3] = cropYMax; + setLimelightNTDoubleArray(limelightName, "crop", entries); } /** - * Gets the horizontal offset with fallback if no target. + * Sets 3D offset point for easy 3D targeting. */ - public static double getTXSafe(String limelightName, double defaultValue) { - if (!hasTarget(limelightName)) { - return defaultValue; - } - return getTX(limelightName); + public static void setFiducial3DOffset(String limelightName, double offsetX, double offsetY, double offsetZ) { + double[] entries = new double[3]; + entries[0] = offsetX; + entries[1] = offsetY; + entries[2] = offsetZ; + setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries); } /** - * Gets the vertical offset with fallback if no target. + * Sets robot orientation values used by MegaTag2 localization algorithm. + * + * @param limelightName Name/identifier of the Limelight + * @param yaw Robot yaw in degrees. 0 = robot facing red alliance wall in FRC + * @param yawRate (Unnecessary) Angular velocity of robot yaw in degrees per second + * @param pitch (Unnecessary) Robot pitch in degrees + * @param pitchRate (Unnecessary) Angular velocity of robot pitch in degrees per second + * @param roll (Unnecessary) Robot roll in degrees + * @param rollRate (Unnecessary) Angular velocity of robot roll in degrees per second */ - public static double getTYSafe(String limelightName, double defaultValue) { - if (!hasTarget(limelightName)) { - return defaultValue; + public static void SetRobotOrientation(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate) { + SetRobotOrientation_INTERNAL(limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, true); + } + + public static void SetRobotOrientation_NoFlush(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate) { + SetRobotOrientation_INTERNAL(limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, false); + } + + private static void SetRobotOrientation_INTERNAL(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate, boolean flush) { + + double[] entries = new double[6]; + entries[0] = yaw; + entries[1] = yawRate; + entries[2] = pitch; + entries[3] = pitchRate; + entries[4] = roll; + entries[5] = rollRate; + setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries); + if(flush) + { + Flush(); } - return getTY(limelightName); } /** - * Gets the target area with fallback if no target. + * Configures the IMU mode for MegaTag2 Localization + * + * @param limelightName Name/identifier of the Limelight + * @param mode IMU mode. */ - public static double getTASafe(String limelightName, double defaultValue) { - if (!hasTarget(limelightName)) { - return defaultValue; - } - return getTA(limelightName); + public static void SetIMUMode(String limelightName, int mode) { + setLimelightNTDouble(limelightName, "imumode_set", mode); } /** - * Calculates the distance to target using pinhole camera model. - * - * @param targetHeightMeters Height of the target above ground - * @param cameraHeightMeters Height of the camera above ground - * @param cameraPitchRadians Camera pitch angle in radians (positive = up) - * @param targetPitchRadians Target pitch from camera in radians - * @return Distance to target in meters + * Sets the 3D point-of-interest offset for the current fiducial pipeline. + * https://docs.limelightvision.io/docs/docs-limelight/pipeline-apriltag/apriltag-3d#point-of-interest-tracking + * + * @param limelightName Name/identifier of the Limelight + * @param x X offset in meters + * @param y Y offset in meters + * @param z Z offset in meters */ - public static double calculateDistanceToTarget(double targetHeightMeters, - double cameraHeightMeters, - double cameraPitchRadians, - double targetPitchRadians) { - double heightDifference = targetHeightMeters - cameraHeightMeters; - double angleToTarget = cameraPitchRadians + targetPitchRadians; - return heightDifference / Math.tan(angleToTarget); + public static void SetFidcuial3DOffset(String limelightName, double x, double y, + double z) { + + double[] entries = new double[3]; + entries[0] = x; + entries[1] = y; + entries[2] = z; + setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries); } /** - * Calculates the horizontal distance to target using the target's vertical offset. - * - * @param limelightName Name of the Limelight - * @param targetHeightMeters Height of the target above ground - * @param cameraHeightMeters Height of the camera above ground - * @param cameraPitchDegrees Camera pitch angle in degrees (positive = up) - * @return Distance to target in meters, or -1 if no target + * Overrides the valid AprilTag IDs that will be used for localization. + * Tags not in this list will be ignored for robot pose estimation. + * + * @param limelightName Name/identifier of the Limelight + * @param validIDs Array of valid AprilTag IDs to track */ - public static double getDistanceToTarget(String limelightName, - double targetHeightMeters, - double cameraHeightMeters, - double cameraPitchDegrees) { - if (!hasTarget(limelightName)) { - return -1.0; + public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) { + double[] validIDsDouble = new double[validIDs.length]; + for (int i = 0; i < validIDs.length; i++) { + validIDsDouble[i] = validIDs[i]; } - - double ty = getTY(limelightName); - double cameraPitchRadians = Math.toRadians(cameraPitchDegrees); - double targetPitchRadians = Math.toRadians(ty); - - return calculateDistanceToTarget(targetHeightMeters, cameraHeightMeters, - cameraPitchRadians, targetPitchRadians); + setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble); } /** - * Takes a snapshot and saves it to the Limelight. - * Useful for debugging or logging. + * Sets the downscaling factor for AprilTag detection. + * Increasing downscale can improve performance at the cost of potentially reduced detection range. + * + * @param limelightName Name/identifier of the Limelight + * @param downscale Downscale factor. Valid values: 1.0 (no downscale), 1.5, 2.0, 3.0, 4.0. Set to 0 for pipeline control. */ - public static void takeSnapshot(String limelightName) { - setLimelightNTDouble(limelightName, "snapshot", 1.0); + public static void SetFiducialDownscalingOverride(String limelightName, float downscale) + { + int d = 0; // pipeline + if (downscale == 1.0) + { + d = 1; + } + if (downscale == 1.5) + { + d = 2; + } + if (downscale == 2) + { + d = 3; + } + if (downscale == 3) + { + d = 4; + } + if (downscale == 4) + { + d = 5; + } + setLimelightNTDouble(limelightName, "fiducial_downscale_set", d); } /** - * Resets the snapshot flag. - */ - public static void resetSnapshot(String limelightName) { - setLimelightNTDouble(limelightName, "snapshot", 0.0); + * Sets the camera pose relative to the robot. + * @param limelightName Name of the Limelight camera + * @param forward Forward offset in meters + * @param side Side offset in meters + * @param up Up offset in meters + * @param roll Roll angle in degrees + * @param pitch Pitch angle in degrees + * @param yaw Yaw angle in degrees + */ + public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, double up, double roll, double pitch, double yaw) { + double[] entries = new double[6]; + entries[0] = forward; + entries[1] = side; + entries[2] = up; + entries[3] = roll; + entries[4] = pitch; + entries[5] = yaw; + setLimelightNTDoubleArray(limelightName, "camerapose_robotspace_set", entries); + } + + ///// + ///// + + public static void setPythonScriptData(String limelightName, double[] outgoingPythonData) { + setLimelightNTDoubleArray(limelightName, "llrobot", outgoingPythonData); + } + + public static double[] getPythonScriptData(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "llpython"); } - // ========== EXAMPLE USAGE ========== + ///// + ///// /** - * Example method showing how to use LimelightHelpers for vision alignment. - * This is just a template - customize for your robot! + * Asynchronously take snapshot. */ - public static class UsageExamples { - - /** - * Example: Get pose estimate and check quality - */ - public static void examplePoseEstimation() { - String limelightName = "limelight"; - - PoseEstimate poseEstimate = getBotPoseEstimate_wpiBlue(limelightName); - - if (poseEstimate != null && poseEstimate.tagCount > 0) { - System.out.println("Robot pose: " + poseEstimate.pose); - System.out.println("Tags seen: " + poseEstimate.tagCount); - System.out.println("Average tag distance: " + poseEstimate.avgTagDist + " m"); - - // Check if pose is reliable (multiple tags, not too far away) - boolean isReliable = poseEstimate.tagCount >= 2 && - poseEstimate.avgTagDist < 5.0 && - poseEstimate.avgTagArea > 0.1; - - if (isReliable) { - System.out.println("Pose estimate is reliable!"); - // Use this pose to update odometry - } + public static CompletableFuture takeSnapshot(String tableName, String snapshotName) { + return CompletableFuture.supplyAsync(() -> { + return SYNCH_TAKESNAPSHOT(tableName, snapshotName); + }); + } + + private static boolean SYNCH_TAKESNAPSHOT(String tableName, String snapshotName) { + URL url = getLimelightURLString(tableName, "capturesnapshot"); + try { + HttpURLConnection connection = (HttpURLConnection) url.openConnection(); + connection.setRequestMethod("GET"); + if (snapshotName != null && snapshotName != "") { + connection.setRequestProperty("snapname", snapshotName); } - } - /** - * Example: Check for specific AprilTag - */ - public static void exampleFiducialDetection() { - String limelightName = "limelight"; - - List fiducials = getRawFiducials(limelightName); - - // Look for a specific tag (e.g., ID 4) - for (RawFiducial fid : fiducials) { - if (fid.id == 4) { - System.out.println("Found tag 4!"); - System.out.println("Distance: " + fid.distToRobot + " m"); - System.out.println("Ambiguity: " + fid.ambiguity); - - // Low ambiguity = good detection - if (fid.ambiguity < 0.2) { - System.out.println("High confidence detection!"); - } - } + int responseCode = connection.getResponseCode(); + if (responseCode == 200) { + return true; + } else { + System.err.println("Bad LL Request"); } + } catch (IOException e) { + System.err.println(e.getMessage()); } + return false; + } - /** - * Example: Configure Limelight for different game modes - */ - public static void exampleConfiguration() { - String limelightName = "limelight"; - - // Autonomous: Use AprilTags with LEDs off - setPipelineIndex(limelightName, 0); - setLEDMode_ForceOff(limelightName); - - // Teleop: Use retroreflective targets with LEDs on - setPipelineIndex(limelightName, 1); - setLEDMode_ForceOn(limelightName); - - // Driver mode: Turn off processing to save CPU - setPipelineIndex(limelightName, 9); // Assume pipeline 9 is driver camera - setLEDMode_ForceOff(limelightName); + /** + * Gets the latest JSON results output and returns a LimelightResults object. + * @param limelightName Name of the Limelight camera + * @return LimelightResults object containing all current target data + */ + public static LimelightResults getLatestResults(String limelightName) { + + long start = System.nanoTime(); + LimelightHelpers.LimelightResults results = new LimelightHelpers.LimelightResults(); + if (mapper == null) { + mapper = new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false); } - /** - * Example: Basic targeting for simple aiming - */ - public static void exampleSimpleTargeting() { - String limelightName = "limelight"; - - if (hasTarget(limelightName)) { - double tx = getTX(limelightName); - double ty = getTY(limelightName); - double area = getTA(limelightName); - - System.out.println("Target found!"); - System.out.println("Horizontal offset: " + tx + " degrees"); - System.out.println("Vertical offset: " + ty + " degrees"); - System.out.println("Area: " + area + "%"); - - // Use tx to aim the robot - // Use ty or area to determine distance - } else { - System.out.println("No target found"); - } + try { + results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); + } catch (JsonProcessingException e) { + results.error = "lljson error: " + e.getMessage(); } - /** - * Example: Using JSON results for complete data - */ - public static void exampleJSONResults() { - String limelightName = "limelight"; - - LimelightResults results = getLatestResults(limelightName); - - if (results.targetingResults.valid == 1) { - // Access all detections - for (FiducialResult fid : results.targetingResults.fiducialResults) { - System.out.println("Fiducial ID " + fid.fiducialID + - " at (" + fid.tx + ", " + fid.ty + ")"); - } - - for (DetectorResult det : results.targetingResults.detectorResults) { - System.out.println("Detected " + det.className + - " with " + (det.confidence * 100) + "% confidence"); - } - } - - printResults(results); + long end = System.nanoTime(); + double millis = (end - start) * .000001; + results.latency_jsonParse = millis; + if (profileJSON) { + System.out.printf("lljson: %.2f\r\n", millis); } + + return results; } } \ No newline at end of file From 7013e89de3a93a37ce937a52451b8042f4694cce Mon Sep 17 00:00:00 2001 From: Matthew Wheeler Date: Sat, 3 Jan 2026 15:34:01 -0800 Subject: [PATCH 3/6] Update test harness to use VisionSubsystem class for testing --- .../commands/test/VisionTestHarness.java | 53 ++++++----------- .../frc/robot/subsystems/VisionSubsystem.java | 58 +++++++++++++++---- 2 files changed, 66 insertions(+), 45 deletions(-) diff --git a/src/main/java/frc/robot/commands/test/VisionTestHarness.java b/src/main/java/frc/robot/commands/test/VisionTestHarness.java index 1400d2b..025684f 100644 --- a/src/main/java/frc/robot/commands/test/VisionTestHarness.java +++ b/src/main/java/frc/robot/commands/test/VisionTestHarness.java @@ -1,54 +1,39 @@ package frc.robot.commands.test; -import edu.wpi.first.networktables.NetworkTableEvent; import edu.wpi.first.networktables.NetworkTableInstance; +import frc.robot.subsystems.VisionSubsystem; public class VisionTestHarness { public static void main(String[] args) { edu.wpi.first.hal.HAL.initialize(500, 0); - NetworkTableInstance nt = NetworkTableInstance.getDefault(); - // Stop client mode and start as the Server + // Start as the Server so the Limelight Client can connect + NetworkTableInstance nt = NetworkTableInstance.getDefault(); nt.stopClient(); nt.startServer(); - System.out.println("Laptop is now the NT Server. Waiting for Limelight..."); + // Instantiate the subsystem + VisionSubsystem vision = new VisionSubsystem(); - // 2025-style Connection Listener - nt.addConnectionListener(true, event -> { - if (event.is(NetworkTableEvent.Kind.kConnected)) { - System.out.println("[CONNECTED] Limelight has joined the table!"); - } - }); + System.out.println("--- Server Started: Waiting for Limelight 10.95.84.11 ---"); while (true) { - var table = nt.getTable("limelight"); - - // tv: 1 if a target is visible, 0 if not - boolean hasTarget = table.getEntry("tv").getDouble(0.0) == 1.0; - - if (hasTarget) { - // Horizontal offset (tx) and Vertical offset (ty) - double tx = table.getEntry("tx").getDouble(0.0); - double ty = table.getEntry("ty").getDouble(0.0); - - // tclass: The string label of the detected object (e.g., "cube", "cone") - String detectedClass = table.getEntry("tclass").getString("None"); - String rawJson = table.getEntry("json").getString(""); - if (!rawJson.isEmpty()) { - System.out.println("RAW DATA: " + rawJson); + if (nt.getConnections().length > 0) { + if (vision.hasTarget()) { + System.out.printf("[LOG] Seeing: %s | TX: %.2f | TY: %.2f | Area: %.2f%%\n", + vision.getDetectedClass(), + vision.getTX(), + vision.getTY(), + vision.getTargetArea() + ); + } else { + System.out.println("[LOG] No targets visible."); } - - // ta: The target area (0% to 100% of the image) - double area = table.getEntry("ta").getDouble(0.0); - - System.out.printf("[DETECTED] Class: %s | TX: %.2f | TY: %.2f | Area: %.2f%%\n", - detectedClass, tx, ty, area); } else { - System.out.println("Searching for objects..."); + System.out.println("[LOG] Waiting for connection..."); } - try { Thread.sleep(200); } catch (InterruptedException e) {} // 5Hz log rate + try { Thread.sleep(500); } catch (InterruptedException e) {} } } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java index e225ade..48bb077 100644 --- a/src/main/java/frc/robot/subsystems/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSubsystem.java @@ -1,32 +1,68 @@ package frc.robot.subsystems; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.subsystems.Limelights.LimelightHelpers; // Note your specific path +import frc.robot.subsystems.Limelights.LimelightHelpers; public class VisionSubsystem extends SubsystemBase { private final String llName = "limelight"; + private NetworkTable table; - public VisionSubsystem() {} - // Get the horizontal offset to the target + public VisionSubsystem() { + this.table = NetworkTableInstance.getDefault().getTable(llName); + } + + /** + * @return true if the Limelight sees any valid target. + */ + public boolean hasTarget() { + return LimelightHelpers.getTV(llName); + } + + /** + * Use this for "Chasing" game pieces. + * @return The class name of the primary detected object (e.g., "Algae"). + */ + public String getDetectedClass() { + String rawJson = table.getEntry("json").getString(""); + if (!rawJson.isEmpty()) { + System.out.println("RAW DATA: " + rawJson); + } + var results = LimelightHelpers.getLatestResults(llName).targets_Detector; + // Your JSON shows data in targets_Detector + if (results != null && results.length > 0) { + // Use .get(0) to access the primary target's data + return results[0].className; + } return "None"; + } + + /** + * Use this for "Shooting" alignment. + * @return Horizontal offset in degrees. + */ public double getTX() { return LimelightHelpers.getTX(llName); } - // Get the vertical offset (useful for distance estimation) + /** + * Use this for distance estimation to the target. + */ public double getTY() { return LimelightHelpers.getTY(llName); } - // Identify the YOLOv8 class currently being seen - public String getDetectedClass() { - return !LimelightHelpers.getNeuralClassID(llName).isEmpty() ? - LimelightHelpers.getLatestResults(llName).targetingResults.classifierResults.get(0).className : - "None"; + /** + * Returns the area of the target as a percentage of the image. + * Useful for determining if you are "close enough" to intake. + */ + public double getTargetArea() { + return LimelightHelpers.getTA(llName); } @Override public void periodic() { - // This runs 50 times a second on the robot + // Optional: Post results to SmartDashboard for driver feedback } -} \ No newline at end of file +} From 4b9edb1d3064e6afccd21f09a93d953e48a0b558 Mon Sep 17 00:00:00 2001 From: Matthew Wheeler Date: Sat, 3 Jan 2026 16:40:24 -0800 Subject: [PATCH 4/6] Add multi-camera support as well as an API that updates localization based on camera data. --- .../commands/test/VisionTestHarness.java | 27 +++-- .../frc/robot/subsystems/VisionSubsystem.java | 104 ++++++++++-------- 2 files changed, 77 insertions(+), 54 deletions(-) diff --git a/src/main/java/frc/robot/commands/test/VisionTestHarness.java b/src/main/java/frc/robot/commands/test/VisionTestHarness.java index 025684f..4479906 100644 --- a/src/main/java/frc/robot/commands/test/VisionTestHarness.java +++ b/src/main/java/frc/robot/commands/test/VisionTestHarness.java @@ -5,35 +5,46 @@ public class VisionTestHarness { public static void main(String[] args) { + // Must initialize HAL to use WPILib components like NetworkTables edu.wpi.first.hal.HAL.initialize(500, 0); - // Start as the Server so the Limelight Client can connect + // Start as the Server so the Limelight (acting as Client) can connect NetworkTableInstance nt = NetworkTableInstance.getDefault(); nt.stopClient(); nt.startServer(); - // Instantiate the subsystem + // Instantiate the refactored subsystem VisionSubsystem vision = new VisionSubsystem(); - System.out.println("--- Server Started: Waiting for Limelight 10.95.84.11 ---"); + System.out.println("--- NT Server Active ---"); + System.out.println("Check Limelight Dashboard: Set 'Custom NT Server IP' to your Laptop IP"); + System.out.println("Waiting for Limelight connection..."); while (true) { + // Check if any client (the Limelight) has connected to our laptop if (nt.getConnections().length > 0) { if (vision.hasTarget()) { - System.out.printf("[LOG] Seeing: %s | TX: %.2f | TY: %.2f | Area: %.2f%%\n", + // Logs data using the working 2025 targets_Detector APIs + System.out.printf("[LIVE] Object: %-10s | TX: %6.2f | TY: %6.2f | Area: %5.2f%%\n", vision.getDetectedClass(), vision.getTX(), vision.getTY(), vision.getTargetArea() ); } else { - System.out.println("[LOG] No targets visible."); + System.out.println("[LIVE] Searching... (No targets in view)"); } } else { - System.out.println("[LOG] Waiting for connection..."); + // Helps debug network location/static IP issues + System.out.println("[OFFLINE] Waiting for Limelight to join NetworkTable..."); } - try { Thread.sleep(500); } catch (InterruptedException e) {} + try { + Thread.sleep(200); // Increased to 5Hz for smoother terminal output + } catch (InterruptedException e) { + Thread.currentThread().interrupt(); + break; + } } } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java index 48bb077..4c56670 100644 --- a/src/main/java/frc/robot/subsystems/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSubsystem.java @@ -1,68 +1,80 @@ package frc.robot.subsystems; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.subsystems.Limelights.LimelightHelpers; public class VisionSubsystem extends SubsystemBase { - private final String llName = "limelight"; - private NetworkTable table; + // LL4 Names - Match these to your Limelight hostnames + private final String objectDetectorName = "limelight"; + private final String[] aprilTagCameras = {"limelight-front", "limelight-back"}; - - public VisionSubsystem() { - this.table = NetworkTableInstance.getDefault().getTable(llName); - } + public VisionSubsystem() {} /** - * @return true if the Limelight sees any valid target. + * Updates the robot's odometry by fusing data from AprilTag cameras using MegaTag2. + * @param drivetrain The drivetrain instance from RobotContainer. */ - public boolean hasTarget() { - return LimelightHelpers.getTV(llName); - } + public void updateOdometry(CommandSwerveDrivetrain drivetrain) { + var driveState = drivetrain.getState(); + double heading = driveState.Pose.getRotation().getDegrees(); + // MegaTag2 requires angular velocity in degrees/sec + double omega = Math.toDegrees(driveState.Speeds.omegaRadiansPerSecond); - /** - * Use this for "Chasing" game pieces. - * @return The class name of the primary detected object (e.g., "Algae"). - */ - public String getDetectedClass() { - String rawJson = table.getEntry("json").getString(""); - if (!rawJson.isEmpty()) { - System.out.println("RAW DATA: " + rawJson); + for (String name : aprilTagCameras) { + // Set orientation for precise 3D localization + LimelightHelpers.SetRobotOrientation(name, heading, omega, 0, 0, 0, 0); + + var alliance = DriverStation.getAlliance(); + LimelightHelpers.PoseEstimate measure; + + // Select the correct alliance pose + if (alliance.isPresent() && alliance.get() == DriverStation.Alliance.Red) { + measure = LimelightHelpers.getBotPoseEstimate_wpiRed_MegaTag2(name); + } else { + measure = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(name); + } + + // Only fuse if we see tags and rotation isn't excessive + if (measure != null && measure.tagCount > 0 && Math.abs(omega) < 720.0) { + // Trust 2+ tags significantly more than 1 tag + double trustFactor = (measure.tagCount >= 2) ? 0.25 : 0.7; + double stdDev = measure.avgTagDist * trustFactor; + + // Call the bridge method added to CommandSwerveDrivetrain + drivetrain.addVisionMeasurement( + measure.pose, + measure.timestampSeconds, + VecBuilder.fill(stdDev, stdDev, 999999) // High theta value to trust gyro + ); + } } - var results = LimelightHelpers.getLatestResults(llName).targets_Detector; - // Your JSON shows data in targets_Detector - if (results != null && results.length > 0) { - // Use .get(0) to access the primary target's data - return results[0].className; - } return "None"; } - /** - * Use this for "Shooting" alignment. - * @return Horizontal offset in degrees. - */ - public double getTX() { - return LimelightHelpers.getTX(llName); - } + // --- Object Detection APIs (Using Working Structure) --- - /** - * Use this for distance estimation to the target. - */ - public double getTY() { - return LimelightHelpers.getTY(llName); + public boolean hasTarget() { + return LimelightHelpers.getTV(objectDetectorName); } - /** - * Returns the area of the target as a percentage of the image. - * Useful for determining if you are "close enough" to intake. - */ - public double getTargetArea() { - return LimelightHelpers.getTA(llName); + public String getDetectedClass() { + // Using the confirmed working API path + var results = LimelightHelpers.getLatestResults(objectDetectorName).targets_Detector; + + if (results != null && results.length > 0) { + return results[0].className; + } + return "None"; } + public double getTX() { return LimelightHelpers.getTX(objectDetectorName); } + public double getTY() { return LimelightHelpers.getTY(objectDetectorName); } + public double getTargetArea() { return LimelightHelpers.getTA(objectDetectorName); } + @Override public void periodic() { - // Optional: Post results to SmartDashboard for driver feedback + // Runs 50Hz on the robot for optional logging } -} +} \ No newline at end of file From edd40a86848ce289e47c119deab35c29634be741 Mon Sep 17 00:00:00 2001 From: Matthew Wheeler Date: Sat, 3 Jan 2026 17:28:26 -0800 Subject: [PATCH 5/6] Add ability to update localization with vision signals. --- src/main/java/frc/robot/Robot.java | 183 ++++-------------- src/main/java/frc/robot/RobotContainer.java | 5 +- .../subsystems/CommandSwerveDrivetrain.java | 17 +- .../subsystems/Limelights/Constants.java | 31 +-- .../frc/robot/subsystems/VisionSubsystem.java | 88 +++++---- 5 files changed, 123 insertions(+), 201 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index c670bc6..e14c332 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1,6 +1,5 @@ package frc.robot; -import edu.wpi.first.math.VecBuilder; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.RobotController; @@ -9,28 +8,28 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import frc.robot.subsystems.Limelights.*; -import frc.robot.subsystems.Limelights.LimelightHelpers.PoseEstimate; +import frc.robot.subsystems.Limelights.Constants; +import frc.robot.subsystems.Limelights.LimelightHelpers; import frc.robot.subsystems.Led.LEDSubsystem; public class Robot extends TimedRobot { private static final double kLowBatteryVoltage = 11.8; - private static final double kLowBatteryDisabledTime = 1.5; //seconds + private static final double kLowBatteryDisabledTime = 1.5; // seconds private final Timer batteryTimer = new Timer(); private boolean lowBatteryAlert = false; - private Command m_autonomousCommand; private final RobotContainer m_container = new RobotContainer(); private final LEDSubsystem m_leds = new LEDSubsystem(); - private static final boolean kUseLimelight = true; // Set to true to enable limelight for now - - private double m_autonomousStartTime = -1.0; + private static final boolean kUseLimelight = true; + /** + * Initial configuration for all Limelights + */ private void configureLimelights() { for (String limelightName : Constants.LimelightConstants.limelightNames) { LimelightHelpers.setLEDMode_PipelineControl(limelightName); @@ -38,91 +37,8 @@ private void configureLimelights() { System.out.println("Configured Limelight: " + limelightName); } } - - private void updateLimelightTelemetry() { - for (String limelightName : Constants.LimelightConstants.limelightNames) { - boolean hasTarget = LimelightHelpers.getTV(limelightName); - SmartDashboard.putBoolean(limelightName + "/HasTarget", hasTarget); - - if (hasTarget) { - SmartDashboard.putNumber(limelightName + "/TX", - LimelightHelpers.getTX(limelightName)); - SmartDashboard.putNumber(limelightName + "/TY", - LimelightHelpers.getTY(limelightName)); - SmartDashboard.putNumber(limelightName + "/TA", - LimelightHelpers.getTA(limelightName)); - SmartDashboard.putNumber(limelightName + "/FiducialID", - LimelightHelpers.getFiducialID(limelightName)); - } - - SmartDashboard.putNumber(limelightName + "/Pipeline", - LimelightHelpers.getCurrentPipelineIndex(limelightName)); - SmartDashboard.putNumber(limelightName + "/Latency", - LimelightHelpers.getLatency_Pipeline(limelightName) + - LimelightHelpers.getLatency_Capture(limelightName)); - } - } - - private void updateVisionMeasurement() { - var driveState = m_container.drivetrain.getState(); - var heading = driveState.Pose.getRotation().getDegrees(); - var omega = driveState.Speeds.omegaRadiansPerSecond; - - for (String limelightName : Constants.LimelightConstants.limelightNames) { - LimelightHelpers.SetRobotOrientation( - limelightName, - heading, - omega * 180.0 / Math.PI, - 0, 0, 0, 0 - ); - - PoseEstimate llMeasurement = null; - var alliance = DriverStation.getAlliance(); - - if (alliance.isPresent()) { - if (alliance.get() == DriverStation.Alliance.Blue) { - llMeasurement = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelightName); - } else { - llMeasurement = LimelightHelpers.getBotPoseEstimate_wpiRed_MegaTag2(limelightName); - } - } else { - llMeasurement = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelightName); - } - - double currentTime = Timer.getFPGATimestamp() * 1000.0; - - if (m_autonomousStartTime < 0 || currentTime - m_autonomousStartTime < 200.0) { - continue; - } - - if (llMeasurement != null && - llMeasurement.tagCount > 0 && - Math.abs(omega) < 2.0) { - - double xyStdDev = llMeasurement.avgTagDist * 0.5; - double rotStdDev = llMeasurement.avgTagDist * 0.5; - - if (llMeasurement.tagCount >= 2) { - xyStdDev *= 0.5; - rotStdDev *= 0.5; - } - - SmartDashboard.putNumber(limelightName + "/VisionTagCount", llMeasurement.tagCount); - SmartDashboard.putNumber(limelightName + "/VisionAvgDist", llMeasurement.avgTagDist); - - m_container.drivetrain.addVisionMeasurement( - llMeasurement.pose, - llMeasurement.timestampSeconds, - VecBuilder.fill(xyStdDev, xyStdDev, rotStdDev) - ); - //TODO: Not sure if this is working or no - } - } - } @Override - public void testPeriodic() {} - public void robotInit() { m_container.robotInit(); @@ -130,86 +46,55 @@ public void robotInit() { configureLimelights(); } - //simulation joystick warning suppression if (RobotBase.isSimulation()) { DriverStation.silenceJoystickConnectionWarning(true); } batteryTimer.start(); - } @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); - // Removed getCANUsagePercent() as it doesn't exist in newer WPILib + // Standard Robot Telemetry SmartDashboard.putNumber("Voltage", RobotController.getBatteryVoltage()); SmartDashboard.putNumber("CPU Temperature", RobotController.getCPUTemp()); SmartDashboard.putBoolean("RSL", RobotController.getRSLState()); SmartDashboard.putNumber("Match Time", DriverStation.getMatchTime()); - SmartDashboard.putNumber("Code Runtime (ms)", Timer.getFPGATimestamp() * 1000.0); - //TODO: Vision updates + // Limelight Updates + // This handles all telemetry and MegaTag2 odometry fusion internally if (kUseLimelight) { - updateLimelightTelemetry(); - updateVisionMeasurement(); - - // NOTE: You need to add LimelightHelpers.java to your project - // Download from: https://github.com/LimelightVision/limelightlib-wpijava - /* - for (String limelightName : Constants.LimelightConstants.limelightNames) { - LimelightHelpers.setRobotOrientation(limelightName, heading, 0, 0, 0, 0, 0); - var llMeasurement = LimelightHelpers.getBotPoseEstimate_wpiBlue(limelightName); - double currentTime = Timer.getFPGATimestamp() * 1000.0; - if (m_autonomousStartTime < 0 || currentTime - m_autonomousStartTime < 200.0) { - // Skip - } else { - if (llMeasurement != null && llMeasurement.tagCount > 0 && Math.abs(omega) < 2) { - m_container.drivetrain.addVisionMeasurement( - llMeasurement.pose, - llMeasurement.timestampSeconds, - new double[]{llMeasurement.avgTagDist * 5, llMeasurement.avgTagDist * 5, llMeasurement.avgTagDist * 5} - ); - } - } - } - */ + m_container.visionSubsystem.updateOdometry(m_container.drivetrain); } - if(DriverStation.isEnabled()){ + // Battery Alert Logic + if (DriverStation.isEnabled()) { batteryTimer.reset(); } - double batteryVoltage = RobotController.getBatteryVoltage(); - - if(batteryVoltage<= kLowBatteryVoltage && batteryTimer.hasElapsed(kLowBatteryDisabledTime)){ - lowBatteryAlert = true; - } else { - lowBatteryAlert = false; - } + lowBatteryAlert = (RobotController.getBatteryVoltage() <= kLowBatteryVoltage + && batteryTimer.hasElapsed(kLowBatteryDisabledTime)); SmartDashboard.putBoolean("LowBattery", lowBatteryAlert); } - @Override - public void disabledInit() { - m_leds.setDisabledMode(); - } - - @Override - public void disabledPeriodic() {} - @Override public void autonomousInit() { - m_autonomousStartTime = Timer.getFPGATimestamp() * 1000.0; + // Signal to VisionSubsystem to start the 200ms safety delay + if (kUseLimelight) { + m_container.visionSubsystem.resetAutoTimer(); + } + m_autonomousCommand = m_container.getAutonomousCommand(); if (m_autonomousCommand != null) { m_autonomousCommand.schedule(); } + m_container.autonomousInit(); m_leds.setAutonomousMode(); - + if (kUseLimelight) { for (String limelightName : Constants.LimelightConstants.limelightNames) { LimelightHelpers.setPipelineIndex(limelightName, 0); @@ -218,17 +103,15 @@ public void autonomousInit() { } } - @Override - public void autonomousPeriodic() {} - @Override public void teleopInit() { if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); } + m_container.teleopInit(); m_leds.setTeleopMode(); - + if (kUseLimelight) { for (String limelightName : Constants.LimelightConstants.limelightNames) { LimelightHelpers.setPipelineIndex(limelightName, 0); @@ -238,17 +121,19 @@ public void teleopInit() { } @Override - public void teleopPeriodic() {} + public void disabledInit() { + m_leds.setDisabledMode(); + } @Override public void testInit() { CommandScheduler.getInstance().cancelAll(); } - @Override - public void simulationPeriodic() { - // This prevents the default message and gives you control - // If you have physics simulation, put it here - // For now: do nothing fast - } -} + // Unused methods kept for structure + @Override public void testPeriodic() {} + @Override public void disabledPeriodic() {} + @Override public void autonomousPeriodic() {} + @Override public void teleopPeriodic() {} + @Override public void simulationPeriodic() {} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3c369d7..b839859 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -27,8 +27,7 @@ import frc.robot.subsystems.AutoCommands.AutoCommands; import frc.robot.subsystems.Climber.Climber; import frc.robot.subsystems.CommandSwerveDrivetrain; -import frc.robot.Superstructure; -import frc.robot.NetworkTables; +import frc.robot.subsystems.VisionSubsystem; public class RobotContainer extends TimedRobot { @@ -53,6 +52,8 @@ public class RobotContainer extends TimedRobot { public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); + public final VisionSubsystem visionSubsystem = new VisionSubsystem(); + public final Climber climber = new Climber(networkTables); private final Superstructure superstructure = new Superstructure(networkTables, drivetrain); private final AutoCommands autoCommands = new AutoCommands(superstructure, networkTables); diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 953500b..157eba2 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -2,9 +2,9 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.Utils; -import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.swerve.SwerveDrivetrain; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; import com.ctre.phoenix6.swerve.SwerveModuleConstants; import com.ctre.phoenix6.swerve.SwerveRequest; @@ -164,4 +164,15 @@ public Command applyRequest(java.util.function.Supplier requestSu public interface DeviceConstructor { BaseStatusSignal create(int deviceId, String canbus); } + + /** + * Injects vision data into the CTRE internal pose estimator. + * * @param pose The robot's pose as seen by the Limelight. + * @param timestamp The timestamp of the measurement in seconds. + * @param stdDevs The trust level (Standard Deviations) for this measurement. + */ + public void addVisionMeasurement(Pose2d pose, double timestamp, Matrix stdDevs) { + // super.addVisionMeasurement is the correct Phoenix 6 API + super.addVisionMeasurement(pose, timestamp, stdDevs); + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Limelights/Constants.java b/src/main/java/frc/robot/subsystems/Limelights/Constants.java index 67ca83d..c7daaf6 100644 --- a/src/main/java/frc/robot/subsystems/Limelights/Constants.java +++ b/src/main/java/frc/robot/subsystems/Limelights/Constants.java @@ -4,24 +4,27 @@ public class Constants { public static class LimelightConstants { - public static final List limelightNames = List.of("limelight-down"); - + // Updated to match your fleet of three Limelight 4s + public static final String OBJECT_DETECTOR_NAME = "limelight"; + public static final List APRILTAG_CAMERA_NAMES = List.of("limelight-front", "limelight-back"); + + // Combined list for initialization loops in Robot.java + public static final List limelightNames = List.of("limelight", "limelight-front", "limelight-back"); + + // Physical Mounting (Update these based on your actual robot CAD) public static final double CAMERA_HEIGHT_METERS = 0.5; public static final double CAMERA_PITCH_DEGREES = 25.0; - + + // Trust and Filtering Constants public static final double VISION_STD_DEV_COEFFICIENT = 0.5; - public static final double MULTI_TAG_STD_DEV_MULTIPLIER = 0.5; - public static final double MAX_ANGULAR_VELOCITY_FOR_VISION = 2.0; - - public static final double MIN_TAG_AREA = 0.1; - public static final double MAX_AMBIGUITY = 0.2; + public static final double MULTI_TAG_STD_DEV_MULTIPLIER = 0.25; // More trust for 2+ tags + + // MegaTag2 is stable at higher speeds than legacy logic + public static final double MAX_ANGULAR_VELOCITY_FOR_VISION_DEG = 720.0; public static final double AUTO_VISION_DELAY_MS = 200.0; - + + // Pipeline IDs (Set in Limelight Web Dashboard) + public static final int NEURAL_DETECTOR_PIPELINE = 0; public static final int APRILTAG_PIPELINE = 0; - public static final int RETROREFLECTIVE_PIPELINE = 1; - public static final int DRIVER_CAMERA_PIPELINE = 9; - - public static final double SPEAKER_TARGET_HEIGHT_METERS = 2.0; - public static final double AMP_TARGET_HEIGHT_METERS = 1.37; } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java index 4c56670..a3e59ca 100644 --- a/src/main/java/frc/robot/subsystems/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSubsystem.java @@ -3,78 +3,100 @@ import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.Limelights.Constants; import frc.robot.subsystems.Limelights.LimelightHelpers; +import java.util.List; + public class VisionSubsystem extends SubsystemBase { - // LL4 Names - Match these to your Limelight hostnames - private final String objectDetectorName = "limelight"; - private final String[] aprilTagCameras = {"limelight-front", "limelight-back"}; + private final String objectDetectorName = Constants.LimelightConstants.OBJECT_DETECTOR_NAME; + private final List aprilTagCameras = Constants.LimelightConstants.APRILTAG_CAMERA_NAMES; + + private double m_autonomousStartTime = -1.0; public VisionSubsystem() {} /** - * Updates the robot's odometry by fusing data from AprilTag cameras using MegaTag2. - * @param drivetrain The drivetrain instance from RobotContainer. + * Call this in autonomousInit() via the Robot class or a Command */ + public void resetAutoTimer() { + m_autonomousStartTime = Timer.getFPGATimestamp() * 1000.0; + } + public void updateOdometry(CommandSwerveDrivetrain drivetrain) { var driveState = drivetrain.getState(); double heading = driveState.Pose.getRotation().getDegrees(); - // MegaTag2 requires angular velocity in degrees/sec double omega = Math.toDegrees(driveState.Speeds.omegaRadiansPerSecond); + double currentTime = Timer.getFPGATimestamp() * 1000.0; + + // Safety 1: Re-implementing the 200ms Auto-Start delay from your Robot.java + if (m_autonomousStartTime > 0 && currentTime - m_autonomousStartTime < 200.0) { + return; + } for (String name : aprilTagCameras) { - // Set orientation for precise 3D localization LimelightHelpers.SetRobotOrientation(name, heading, omega, 0, 0, 0, 0); var alliance = DriverStation.getAlliance(); - LimelightHelpers.PoseEstimate measure; + LimelightHelpers.PoseEstimate measure = (alliance.isPresent() && alliance.get() == DriverStation.Alliance.Red) + ? LimelightHelpers.getBotPoseEstimate_wpiRed_MegaTag2(name) + : LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(name); - // Select the correct alliance pose - if (alliance.isPresent() && alliance.get() == DriverStation.Alliance.Red) { - measure = LimelightHelpers.getBotPoseEstimate_wpiRed_MegaTag2(name); - } else { - measure = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(name); - } - - // Only fuse if we see tags and rotation isn't excessive + // Safety 2: Re-implementing the spin-filter and tag-count check if (measure != null && measure.tagCount > 0 && Math.abs(omega) < 720.0) { - // Trust 2+ tags significantly more than 1 tag - double trustFactor = (measure.tagCount >= 2) ? 0.25 : 0.7; - double stdDev = measure.avgTagDist * trustFactor; - // Call the bridge method added to CommandSwerveDrivetrain + // Safety 3: Re-implementing your Dynamic StdDev logic + // Your old code: xyStdDev = avgTagDist * 0.5; + double trustFactor = (measure.tagCount >= 2) ? 0.25 : 0.5; + double xyStdDev = measure.avgTagDist * trustFactor; + + // We use 999999 for rotation to trust the Pigeon2 gyro over vision drivetrain.addVisionMeasurement( measure.pose, measure.timestampSeconds, - VecBuilder.fill(stdDev, stdDev, 999999) // High theta value to trust gyro + VecBuilder.fill(xyStdDev, xyStdDev, 999999) ); + + // Telemetry: Moved from Robot.java to here + SmartDashboard.putNumber("Vision/" + name + "/TagCount", measure.tagCount); + SmartDashboard.putNumber("Vision/" + name + "/AvgDist", measure.avgTagDist); } } } - // --- Object Detection APIs (Using Working Structure) --- + @Override + public void periodic() { + // Re-implementing updateLimelightTelemetry() from your Robot.java + // This keeps the dashboard updated even if odometry isn't being fused + updateDashboardTelemetry(objectDetectorName); + for(String name : aprilTagCameras) { + updateDashboardTelemetry(name); + } + } + + private void updateDashboardTelemetry(String name) { + boolean hasTarget = LimelightHelpers.getTV(name); + SmartDashboard.putBoolean(name + "/HasTarget", hasTarget); + if (hasTarget) { + SmartDashboard.putNumber(name + "/TX", LimelightHelpers.getTX(name)); + SmartDashboard.putNumber(name + "/TY", LimelightHelpers.getTY(name)); + } + } + // --- Neural Detector Accessors --- public boolean hasTarget() { return LimelightHelpers.getTV(objectDetectorName); } public String getDetectedClass() { - // Using the confirmed working API path var results = LimelightHelpers.getLatestResults(objectDetectorName).targets_Detector; - - if (results != null && results.length > 0) { - return results[0].className; - } - return "None"; + return (results != null && results.length > 0) ? results[0].className : "None"; } public double getTX() { return LimelightHelpers.getTX(objectDetectorName); } public double getTY() { return LimelightHelpers.getTY(objectDetectorName); } public double getTargetArea() { return LimelightHelpers.getTA(objectDetectorName); } - - @Override - public void periodic() { - // Runs 50Hz on the robot for optional logging - } } \ No newline at end of file From d08977b981a947b4dd198b45139f067086a531c5 Mon Sep 17 00:00:00 2001 From: Matthew Wheeler Date: Sat, 3 Jan 2026 17:40:22 -0800 Subject: [PATCH 6/6] An (untested) attempt to build a driving assist command --- src/main/java/frc/robot/RobotContainer.java | 8 ++++ .../java/frc/robot/commands/VisionAlign.java | 43 +++++++++++++++++++ 2 files changed, 51 insertions(+) create mode 100644 src/main/java/frc/robot/commands/VisionAlign.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b839859..36778c1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -23,6 +23,7 @@ import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.commands.VisionAlign; import frc.robot.generated.TunerConstants; import frc.robot.subsystems.AutoCommands.AutoCommands; import frc.robot.subsystems.Climber.Climber; @@ -131,6 +132,13 @@ private void configureBindings() { }) ); + controller.x().whileTrue(new VisionAlign( + drivetrain, + visionSubsystem, + () -> -controller.getLeftY() * MAX_SPEED, + () -> -controller.getLeftX() * MAX_SPEED + )); + new Trigger(controller.a().whileTrue(drivetrain.applyRequest(() -> brake))); new Trigger(controller.b().whileTrue(drivetrain.applyRequest(() -> point.withModuleDirection(new Rotation2d(-controller.getLeftY(), -controller.getLeftX()))))); diff --git a/src/main/java/frc/robot/commands/VisionAlign.java b/src/main/java/frc/robot/commands/VisionAlign.java new file mode 100644 index 0000000..2b624c5 --- /dev/null +++ b/src/main/java/frc/robot/commands/VisionAlign.java @@ -0,0 +1,43 @@ +package frc.robot.commands; + +import java.util.function.DoubleSupplier; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.CommandSwerveDrivetrain; +import frc.robot.subsystems.VisionSubsystem; +import com.ctre.phoenix6.swerve.SwerveRequest; + +public class VisionAlign extends Command { + private final CommandSwerveDrivetrain m_drivetrain; + private final VisionSubsystem m_vision; + private final DoubleSupplier m_throttleX, m_throttleY; + + // PID constants for rotation alignment + private final PIDController m_controller = new PIDController(0.05, 0.0, 0.002); + private final SwerveRequest.FieldCentric m_driveRequest = new SwerveRequest.FieldCentric(); + + public VisionAlign(CommandSwerveDrivetrain drivetrain, VisionSubsystem vision, + DoubleSupplier throttleX, DoubleSupplier throttleY) { + m_drivetrain = drivetrain; + m_vision = vision; + m_throttleX = throttleX; + m_throttleY = throttleY; + addRequirements(m_drivetrain); + } + + @Override + public void execute() { + double rotationOutput = 0; + + if (m_vision.hasTarget()) { + // Error is the horizontal offset from the crosshair + rotationOutput = m_controller.calculate(m_vision.getTX(), 0); + } + + // Apply velocities: X and Y from sticks, Rotation from PID + m_drivetrain.setControl(m_driveRequest + .withVelocityX(m_throttleX.getAsDouble()) + .withVelocityY(m_throttleY.getAsDouble()) + .withRotationalRate(rotationOutput)); + } +} \ No newline at end of file