From b864eb441ed594270bd9979dc37016aaaef88782 Mon Sep 17 00:00:00 2001 From: Adharrsh Date: Thu, 16 Jan 2025 19:15:27 -0500 Subject: [PATCH 1/7] created vision subsystem --- .../subsystems/limelite/LimelightHelpers.java | 1589 +++++++++++++++++ .../frc/robot/subsystems/limelite/vision.java | 89 + 2 files changed, 1678 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/limelite/LimelightHelpers.java create mode 100644 src/main/java/frc/robot/subsystems/limelite/vision.java diff --git a/src/main/java/frc/robot/subsystems/limelite/LimelightHelpers.java b/src/main/java/frc/robot/subsystems/limelite/LimelightHelpers.java new file mode 100644 index 0000000..b2400e3 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/limelite/LimelightHelpers.java @@ -0,0 +1,1589 @@ +//LimelightHelpers v1.10 (REQUIRES LLOS 2024.9.1 OR LATER) + +package frc.robot; + +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.LimelightHelpers.LimelightResults; +import frc.robot.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 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.util.concurrent.ConcurrentHashMap; + +/** + * 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<>(); + + /** + * Represents a Color/Retroreflective Target Result extracted from JSON Output + */ + 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); + } + + 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); + } + + @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; + + @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]; + } + + } + + /** + * Represents an AprilTag/Fiducial Target Result extracted from JSON Output + */ + public static class LimelightTarget_Fiducial { + + @JsonProperty("fID") + public double fiducialID; + + @JsonProperty("fam") + public String fiducialFamily; + + @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); + } + + 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); + } + + @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; + + @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]; + } + } + + /** + * Represents a Barcode Target Result extracted from JSON Output + */ + public static class LimelightTarget_Barcode { + + /** + * Barcode family type (e.g. "QR", "DataMatrix", etc.) + */ + @JsonProperty("fam") + public String family; + + /** + * Gets the decoded data content of the barcode + */ + @JsonProperty("data") + public String data; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("typ") + public double ty_pixels; + + @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; + } + } + + /** + * Represents a Neural Classifier Pipeline Result extracted from JSON Output + */ + 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() { + } + } + + /** + * Represents a Neural Detector Pipeline Result extracted from JSON Output + */ + 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() { + } + } + + /** + * Limelight Results object, parsed from a Limelight's JSON results output. + */ + 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]; + + } + + + } + + /** + * Represents a Limelight Raw Fiducial result from Limelight's NetworkTables output. + */ + 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; + } + } + + /** + * Represents a Limelight Raw Neural Detector result from Limelight's NetworkTables output. + */ + 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; + } + } + + /** + * Represents a 3D Pose Estimate. + */ + 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 RawFiducial[] rawFiducials; + public boolean isMegaTag2; + + /** + * 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; + } + + 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; + } + + } + + private static ObjectMapper mapper; + + /** + * Print JSON Parse time to the console in milliseconds + */ + static boolean profileJSON = false; + + static final String sanitizeName(String name) { + if (name == "" || name == null) { + return "limelight"; + } + return name; + } + + /** + * 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 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]))); + } + + /** + * 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 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]; + } + + 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; + + if (poseArray.length == 0) { + // Handle the case where no data is available + return null; // or some default PoseEstimate + } + + 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); + + // Convert server timestamp from microseconds to seconds and adjust for latency + double adjustedTimestamp = (timestamp / 1000000.0) - (latency / 1000.0); + + 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 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 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 RawFiducial[0]; + } + + int numFiducials = rawFiducialArray.length / valsPerEntry; + RawFiducial[] rawFiducials = new RawFiducial[numFiducials]; + + for (int i = 0; i < numFiducials; i++) { + int baseIndex = i * valsPerEntry; + int id = (int) extractArrayEntry(rawFiducialArray, baseIndex); + double txnc = extractArrayEntry(rawFiducialArray, baseIndex + 1); + double tync = extractArrayEntry(rawFiducialArray, baseIndex + 2); + double ta = extractArrayEntry(rawFiducialArray, baseIndex + 3); + double distToCamera = extractArrayEntry(rawFiducialArray, baseIndex + 4); + double distToRobot = extractArrayEntry(rawFiducialArray, baseIndex + 5); + double ambiguity = extractArrayEntry(rawFiducialArray, baseIndex + 6); + + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + + return rawFiducials; + } + + /** + * 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 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 RawDetection[0]; + } + + int numDetections = rawDetectionArray.length / valsPerEntry; + RawDetection[] rawDetections = new RawDetection[numDetections]; + + for (int i = 0; i < numDetections; i++) { + 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); + double ta = extractArrayEntry(rawDetectionArray, baseIndex + 3); + double corner0_X = extractArrayEntry(rawDetectionArray, baseIndex + 4); + double corner0_Y = extractArrayEntry(rawDetectionArray, baseIndex + 5); + double corner1_X = extractArrayEntry(rawDetectionArray, baseIndex + 6); + double corner1_Y = extractArrayEntry(rawDetectionArray, baseIndex + 7); + double corner2_X = extractArrayEntry(rawDetectionArray, baseIndex + 8); + double corner2_Y = extractArrayEntry(rawDetectionArray, baseIndex + 9); + double corner3_X = extractArrayEntry(rawDetectionArray, baseIndex + 10); + double corner3_Y = extractArrayEntry(rawDetectionArray, baseIndex + 11); + + 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; + } + + /** + * 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; + } + ///// + ///// + + /** + * 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 boolean getTV(String limelightName) { + return 1.0 == getLimelightNTDouble(limelightName, "tv"); + } + + /** + * 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; + } + + /** + * 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 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 + */ + public static int getDetectorClassIndex (String limelightName) { + double[] t2d = getT2DArray(limelightName); + if(t2d.length == 17) + { + return (int)t2d[11]; + } + return 0; + } + + /** + * 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"); + } + + /** + * 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"); + } + + /** + * 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"); + } + + /** + * 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"); + } + + /** + * 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 current pipeline type. + * @param limelightName Name of the Limelight camera + * @return Pipeline type string (e.g. "retro", "apriltag", etc) + */ + public static String getCurrentPipelineType(String limelightName) { + return getLimelightNTString(limelightName, "getpipetype"); + } + + /** + * Gets the full JSON results dump. + * @param limelightName Name of the Limelight camera + * @return JSON string containing all current results + */ + public static String getJSONDump(String limelightName) { + return getLimelightNTString(limelightName, "json"); + } + + /** + * Switch to getBotPose + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + /** + * Switch to getBotPose_wpiRed + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + /** + * Switch to getBotPose_wpiBlue + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + public static double[] getBotPose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + public static double[] getBotPose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + } + + public static double[] getCameraPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + } + + public static double[] getTargetPose_CameraSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + } + + public static double[] getTargetPose_RobotSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + } + + public static double[] getTargetColor(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "tc"); + } + + public static double getFiducialID(String limelightName) { + return getLimelightNTDouble(limelightName, "tid"); + } + + public static String getNeuralClassID(String limelightName) { + return getLimelightNTString(limelightName, "tclass"); + } + + public static String[] getRawBarcodeData(String limelightName) { + return getLimelightNTStringArray(limelightName, "rawbarcodes"); + } + + ///// + ///// + + public static Pose3d getBotPose3d(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose"); + return toPose3D(poseArray); + } + + /** + * (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); + } + + /** + * (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); + } + + /** + * 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); + } + + /** + * 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); + } + + /** + * 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); + } + + /** + * 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); + } + + /** + * 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); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiBlue(String limelightName) { + + double[] result = getBotPose_wpiBlue(limelightName); + return toPose2D(result); + } + + /** + * 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); + } + + /** + * 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); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiRed(String limelightName) { + + double[] result = getBotPose_wpiRed(limelightName); + return toPose2D(result); + + } + + /** + * 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); + } + + /** + * 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); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d(String limelightName) { + + double[] result = getBotPose(limelightName); + return toPose2D(result); + + } + + + + ///// + ///// + + public static void setPipelineIndex(String limelightName, int pipelineIndex) { + setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); + } + + + public static void setPriorityTagID(String limelightName, int ID) { + setLimelightNTDouble(limelightName, "priorityid", ID); + } + + /** + * 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); + } + + public static void setLEDMode_ForceOff(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 1); + } + + public static void setLEDMode_ForceBlink(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 2); + } + + public static void setLEDMode_ForceOn(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 3); + } + + /** + * Enables standard side-by-side stream mode. + * @param limelightName Name of the Limelight camera + */ + public static void setStreamMode_Standard(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 0); + } + + /** + * Enables Picture-in-Picture mode with secondary stream in the corner. + * @param limelightName Name of the Limelight camera + */ + public static void setStreamMode_PiPMain(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 1); + } + + /** + * Enables Picture-in-Picture mode with primary stream in the corner. + * @param limelightName Name of the Limelight camera + */ + public static void setStreamMode_PiPSecondary(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 2); + } + + + /** + * 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 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); + } + + /** + * Sets 3D offset point for easy 3D targeting. + */ + 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); + } + + /** + * 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 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(); + } + } + + /** + * 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 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); + } + + /** + * 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 void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) { + double[] validIDsDouble = new double[validIDs.length]; + for (int i = 0; i < validIDs.length; i++) { + validIDsDouble[i] = validIDs[i]; + } + setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble); + } + + /** + * 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 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); + } + + /** + * 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"); + } + + ///// + ///// + + /** + * Asynchronously take snapshot. + */ + 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); + } + + 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; + } + + /** + * 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); + } + + try { + results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); + } catch (JsonProcessingException e) { + results.error = "lljson error: " + e.getMessage(); + } + + 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; + } +} diff --git a/src/main/java/frc/robot/subsystems/limelite/vision.java b/src/main/java/frc/robot/subsystems/limelite/vision.java new file mode 100644 index 0000000..8140d3a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/limelite/vision.java @@ -0,0 +1,89 @@ +package frc.robot; + +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.NetworkTableEntry; + +public class vision { + private static NetworkTableInstance table = null; + + + //Light modes for Limelight. + public static enum LightMode { + eOn, eOff, eBlink + } + + //Camera modes for Limelight. + public static enum CameraMode { + eVision, eDriver + } + + //is there a target in the area + public static boolean isTarget() { + return getValue("tv").getDouble(0) == 1; + } + + //Horizontal offset from crosshair to target + public static double getTx() { + return getValue("tx").getDouble(0.00); + } + + //Vertical offset from crosshair to target + public static double getTy() { + return getValue("ty").getDouble(0.00); + } + + //Area that the detected target takes up in total camera FOV + public static double getTa() { + return getValue("ta").getDouble(0.00); + } + + //Gets target skew or rotation + public static double getTs() { + return getValue("ts").getDouble(0.00); + } + + //Gets target latency (ms) + public static double getTl() { + return getValue("tl").getDouble(0.00); + } + + //Sets LED mode of Limelight. + public static void setLedMode(LightMode mode) { + getValue("ledMode").setNumber(mode.ordinal()); + } + + //Sets camera mode for Limelight. + public static void setCameraMode(CameraMode mode) { + getValue("camMode").setNumber(mode.ordinal()); + } + + //Sets pipeline number + public static void setPipeline(int number) { + getValue("pipeline").setNumber(number); + } + + public static void main(String[] args) { + // Set the LED mode to blink + setLedMode(LightMode.eBlink); + System.out.println("Limelight LEDs should now be blinking."); + } + + //Helper method to get an entry from the Limelight NetworkTable. + private static NetworkTableEntry getValue(String key) { + if (table == null) { + table = NetworkTableInstance.getDefault(); + } + + return table.getTable("limelight").getEntry(key); + } + + public static void updateLedMode(boolean isTargetDetected, boolean isInAutoMode) { + if (isInAutoMode) { + setLedMode(LightMode.eOn); + } else if (isTargetDetected) { + setLedMode(LightMode.eBlink); + } else { + setLedMode(LightMode.eOff); + } + } +} \ No newline at end of file From 0ee5ebafc3c267dded0849263497af373b407ded Mon Sep 17 00:00:00 2001 From: Adharrsh Date: Thu, 16 Jan 2025 19:17:36 -0500 Subject: [PATCH 2/7] updated vision --- src/main/java/frc/robot/RobotContainer.java | 8 ++++++++ .../robot/subsystems/limelite/LimelightHelpers.java | 4 +--- .../java/frc/robot/subsystems/limelite/vision.java | 10 ++++------ 3 files changed, 13 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a65ad18..eb893cb 100755 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -10,6 +10,7 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.constants.Constants; +import frc.robot.subsystems.limelite.vision; /** @@ -28,6 +29,7 @@ public class RobotContainer { public RobotContainer() { xboxController = new XboxController(Constants.XBOX_CONTROLLER_PORT); configureBindings(); + updateLimeLightStatus(); } @@ -52,4 +54,10 @@ private void configureBindings() { public Command getAutonomousCommand() { return null; } + + private void updateLimeLightStatus(){ + boolean isTargetDetected = vision.isTarget(); + vision.updateLedMode(isTargetDetected); + } } + diff --git a/src/main/java/frc/robot/subsystems/limelite/LimelightHelpers.java b/src/main/java/frc/robot/subsystems/limelite/LimelightHelpers.java index b2400e3..989bd5f 100644 --- a/src/main/java/frc/robot/subsystems/limelite/LimelightHelpers.java +++ b/src/main/java/frc/robot/subsystems/limelite/LimelightHelpers.java @@ -1,14 +1,12 @@ //LimelightHelpers v1.10 (REQUIRES LLOS 2024.9.1 OR LATER) -package frc.robot; +package frc.robot.subsystems.limelite; 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.LimelightHelpers.LimelightResults; -import frc.robot.LimelightHelpers.PoseEstimate; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; diff --git a/src/main/java/frc/robot/subsystems/limelite/vision.java b/src/main/java/frc/robot/subsystems/limelite/vision.java index 8140d3a..b3691da 100644 --- a/src/main/java/frc/robot/subsystems/limelite/vision.java +++ b/src/main/java/frc/robot/subsystems/limelite/vision.java @@ -1,4 +1,4 @@ -package frc.robot; +package frc.robot.subsystems.limelite; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.NetworkTableEntry; @@ -9,7 +9,7 @@ public class vision { //Light modes for Limelight. public static enum LightMode { - eOn, eOff, eBlink + eOff, eBlink } //Camera modes for Limelight. @@ -77,10 +77,8 @@ private static NetworkTableEntry getValue(String key) { return table.getTable("limelight").getEntry(key); } - public static void updateLedMode(boolean isTargetDetected, boolean isInAutoMode) { - if (isInAutoMode) { - setLedMode(LightMode.eOn); - } else if (isTargetDetected) { + public static void updateLedMode(boolean isTargetDetected) { + if (isTargetDetected) { setLedMode(LightMode.eBlink); } else { setLedMode(LightMode.eOff); From 1fb59c5d7e8cef45728929be898ff41ecb170bb7 Mon Sep 17 00:00:00 2001 From: Adharrsh Date: Sat, 18 Jan 2025 11:04:35 -0500 Subject: [PATCH 3/7] added buttons --- src/main/java/frc/robot/RobotContainer.java | 39 +++++++++++++++++---- 1 file changed, 32 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index eb893cb..c08a2d5 100755 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -5,13 +5,16 @@ package frc.robot; +import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import edu.wpi.first.wpilibj2.command.button.Trigger; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.button.*; import frc.robot.constants.Constants; import frc.robot.subsystems.limelite.vision; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import java.util.function.BooleanSupplier; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -20,6 +23,7 @@ * subsystems, commands, and trigger mappings) should be declared here. */ public class RobotContainer { + private final Joystick driverJoystick = new Joystick(0); XboxController xboxController; @@ -29,21 +33,22 @@ public class RobotContainer { public RobotContainer() { xboxController = new XboxController(Constants.XBOX_CONTROLLER_PORT); configureBindings(); - updateLimeLightStatus(); } /** * Use this method to define your trigger->command mappings. Triggers can be created via the - * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with an arbitrary + * {@link Trigger#Trigger(BooleanSupplier)} constructor with an arbitrary * predicate, or via the named factories in {@link - * edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses for {@link - * CommandXboxController Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller - * PS4} controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick Flight + * CommandGenericHID}'s subclasses for {@link + * CommandXboxController Xbox}/{@link CommandPS4Controller + * PS4} controllers or {@link CommandJoystick Flight * joysticks}. */ private void configureBindings() { + new JoystickButton(driverJoystick, 1).onTrue(new InstantCommand(this::updateLimeLightStatus)); + new JoystickButton(driverJoystick, 2).onTrue(new InstantCommand(this::adjustPositionOfCrosshair)); } /** @@ -59,5 +64,25 @@ private void updateLimeLightStatus(){ boolean isTargetDetected = vision.isTarget(); vision.updateLedMode(isTargetDetected); } + + private void adjustPositionOfCrosshair(){ + double tx = vision.getTx(); + double ty = vision.getTy(); + + if (Math.abs(tx) > 1.00){ + System.out.println("Crosshair IS NOT aligned horizontally by [ " + tx + " ]"); + } + else{ + System.out.println("Crosshair IS aligned horizontally by [ " + tx + " ]"); + } + + if (Math.abs(ty) > 1.00){ + System.out.println("Crosshair IS NOT aligned vertically by [ " + ty + " ]"); + } + else{ + System.out.println("Crosshair IS aligned horizontally by [ " + ty + " ]"); + } + + } } From fde11faa10492fed3aff4bd70bbf01a2c8a6b284 Mon Sep 17 00:00:00 2001 From: Adharrsh Date: Sat, 25 Jan 2025 11:02:19 -0500 Subject: [PATCH 4/7] Made a new limelight system with limeliteHelpers --- src/main/java/frc/robot/RobotContainer.java | 4 +- .../subsystems/limelite/LimelightHelpers.java | 5 + .../frc/robot/subsystems/limelite/Vision.java | 4 + .../frc/robot/subsystems/limelite/vision.java | 110 +++++------------- 4 files changed, 40 insertions(+), 83 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/limelite/Vision.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c08a2d5..26c2c6a 100755 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -46,9 +46,9 @@ public RobotContainer() { * joysticks}. */ private void configureBindings() { - new JoystickButton(driverJoystick, 1).onTrue(new InstantCommand(this::updateLimeLightStatus)); + new JoystickButton(driverJoystick, XboxController.Button.kA.value).onTrue(new InstantCommand(this::updateLimeLightStatus)); - new JoystickButton(driverJoystick, 2).onTrue(new InstantCommand(this::adjustPositionOfCrosshair)); + new JoystickButton(driverJoystick, XboxController.Button.kB.value).onTrue(new InstantCommand(this::adjustPositionOfCrosshair)); } /** diff --git a/src/main/java/frc/robot/subsystems/limelite/LimelightHelpers.java b/src/main/java/frc/robot/subsystems/limelite/LimelightHelpers.java index 989bd5f..afb26bc 100644 --- a/src/main/java/frc/robot/subsystems/limelite/LimelightHelpers.java +++ b/src/main/java/frc/robot/subsystems/limelite/LimelightHelpers.java @@ -28,6 +28,8 @@ import com.fasterxml.jackson.core.JsonProcessingException; import com.fasterxml.jackson.databind.DeserializationFeature; import com.fasterxml.jackson.databind.ObjectMapper; +import edu.wpi.first.wpilibj.Joystick; + import java.util.concurrent.ConcurrentHashMap; /** @@ -407,6 +409,7 @@ public static class LimelightResults { @JsonProperty("t6c_rs") public double[] camerapose_robotspace; + public LimelightResults results; public Pose3d getBotPose3d() { return toPose3D(botpose); @@ -461,6 +464,8 @@ public LimelightResults() { } + public Joystick getBestTarget() { + } } /** diff --git a/src/main/java/frc/robot/subsystems/limelite/Vision.java b/src/main/java/frc/robot/subsystems/limelite/Vision.java new file mode 100644 index 0000000..184d8c2 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/limelite/Vision.java @@ -0,0 +1,4 @@ +package frc.robot.subsystems.limelite; + +public class Vision { +} diff --git a/src/main/java/frc/robot/subsystems/limelite/vision.java b/src/main/java/frc/robot/subsystems/limelite/vision.java index b3691da..03f8d74 100644 --- a/src/main/java/frc/robot/subsystems/limelite/vision.java +++ b/src/main/java/frc/robot/subsystems/limelite/vision.java @@ -1,87 +1,35 @@ package frc.robot.subsystems.limelite; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.networktables.NetworkTableEntry; - -public class vision { - private static NetworkTableInstance table = null; - - - //Light modes for Limelight. - public static enum LightMode { - eOff, eBlink - } - - //Camera modes for Limelight. - public static enum CameraMode { - eVision, eDriver - } - - //is there a target in the area - public static boolean isTarget() { - return getValue("tv").getDouble(0) == 1; - } - - //Horizontal offset from crosshair to target - public static double getTx() { - return getValue("tx").getDouble(0.00); - } - - //Vertical offset from crosshair to target - public static double getTy() { - return getValue("ty").getDouble(0.00); - } - - //Area that the detected target takes up in total camera FOV - public static double getTa() { - return getValue("ta").getDouble(0.00); - } - - //Gets target skew or rotation - public static double getTs() { - return getValue("ts").getDouble(0.00); - } - - //Gets target latency (ms) - public static double getTl() { - return getValue("tl").getDouble(0.00); - } - - //Sets LED mode of Limelight. - public static void setLedMode(LightMode mode) { - getValue("ledMode").setNumber(mode.ordinal()); - } - - //Sets camera mode for Limelight. - public static void setCameraMode(CameraMode mode) { - getValue("camMode").setNumber(mode.ordinal()); - } - - //Sets pipeline number - public static void setPipeline(int number) { - getValue("pipeline").setNumber(number); - } - +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.limelite.LimelightHelpers; +import frc.robot.subsystems.limelite.LimelightHelpers.LimelightResults; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; + +public class Vision { public static void main(String[] args) { - // Set the LED mode to blink - setLedMode(LightMode.eBlink); - System.out.println("Limelight LEDs should now be blinking."); - } - - //Helper method to get an entry from the Limelight NetworkTable. - private static NetworkTableEntry getValue(String key) { - if (table == null) { - table = NetworkTableInstance.getDefault(); - } - - return table.getTable("limelight").getEntry(key); - } - - public static void updateLedMode(boolean isTargetDetected) { - if (isTargetDetected) { - setLedMode(LightMode.eBlink); + // Fetch the latest results from the Limelight + LimelightResults results = LimelightHelpers.getLatestResults("limelight"); + + // Check the number of detected April Tags + int numAprilTags = results.targets_Fiducials.length; + System.out.println("Number of April Tags detected: " + numAprilTags); + + if (numAprilTags > 0) { + // Assuming we are interested in the first detected April Tag + LimelightHelpers.LimelightTarget_Fiducial target = results.targets_Fiducials[0]; + + // Extract and print the pose and other relevant data + System.out.println("April Tag ID: " + target.fiducialID); + System.out.println("Pose (Field Space): " + target.getRobotPose_FieldSpace2D()); + System.out.println("Target Area: " + target.ta); + System.out.println("Horizontal Offset: " + target.tx); + System.out.println("Vertical Offset: " + target.ty); } else { - setLedMode(LightMode.eOff); + System.out.println("No April Tags detected."); } } -} \ No newline at end of file +} From db4f6a6bd215bb6bb31bccd31c1ab14f95b34464 Mon Sep 17 00:00:00 2001 From: Adharrsh Date: Sat, 25 Jan 2025 11:04:14 -0500 Subject: [PATCH 5/7] Made a new limelight system with limeliteHelpers --- .../frc/robot/subsystems/limelite/Vision.java | 31 ++++++++++++++++ .../frc/robot/subsystems/limelite/vision.java | 35 ------------------- 2 files changed, 31 insertions(+), 35 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/limelite/vision.java diff --git a/src/main/java/frc/robot/subsystems/limelite/Vision.java b/src/main/java/frc/robot/subsystems/limelite/Vision.java index 184d8c2..03f8d74 100644 --- a/src/main/java/frc/robot/subsystems/limelite/Vision.java +++ b/src/main/java/frc/robot/subsystems/limelite/Vision.java @@ -1,4 +1,35 @@ package frc.robot.subsystems.limelite; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.limelite.LimelightHelpers; +import frc.robot.subsystems.limelite.LimelightHelpers.LimelightResults; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; + public class Vision { + public static void main(String[] args) { + // Fetch the latest results from the Limelight + LimelightResults results = LimelightHelpers.getLatestResults("limelight"); + + // Check the number of detected April Tags + int numAprilTags = results.targets_Fiducials.length; + System.out.println("Number of April Tags detected: " + numAprilTags); + + if (numAprilTags > 0) { + // Assuming we are interested in the first detected April Tag + LimelightHelpers.LimelightTarget_Fiducial target = results.targets_Fiducials[0]; + + // Extract and print the pose and other relevant data + System.out.println("April Tag ID: " + target.fiducialID); + System.out.println("Pose (Field Space): " + target.getRobotPose_FieldSpace2D()); + System.out.println("Target Area: " + target.ta); + System.out.println("Horizontal Offset: " + target.tx); + System.out.println("Vertical Offset: " + target.ty); + } else { + System.out.println("No April Tags detected."); + } + } } diff --git a/src/main/java/frc/robot/subsystems/limelite/vision.java b/src/main/java/frc/robot/subsystems/limelite/vision.java deleted file mode 100644 index 03f8d74..0000000 --- a/src/main/java/frc/robot/subsystems/limelite/vision.java +++ /dev/null @@ -1,35 +0,0 @@ -package frc.robot.subsystems.limelite; - -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.subsystems.limelite.LimelightHelpers; -import frc.robot.subsystems.limelite.LimelightHelpers.LimelightResults; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; - -public class Vision { - public static void main(String[] args) { - // Fetch the latest results from the Limelight - LimelightResults results = LimelightHelpers.getLatestResults("limelight"); - - // Check the number of detected April Tags - int numAprilTags = results.targets_Fiducials.length; - System.out.println("Number of April Tags detected: " + numAprilTags); - - if (numAprilTags > 0) { - // Assuming we are interested in the first detected April Tag - LimelightHelpers.LimelightTarget_Fiducial target = results.targets_Fiducials[0]; - - // Extract and print the pose and other relevant data - System.out.println("April Tag ID: " + target.fiducialID); - System.out.println("Pose (Field Space): " + target.getRobotPose_FieldSpace2D()); - System.out.println("Target Area: " + target.ta); - System.out.println("Horizontal Offset: " + target.tx); - System.out.println("Vertical Offset: " + target.ty); - } else { - System.out.println("No April Tags detected."); - } - } -} From c3cd30504001794d812c118e35583d66f10241be Mon Sep 17 00:00:00 2001 From: Adharrsh Date: Sat, 25 Jan 2025 11:07:57 -0500 Subject: [PATCH 6/7] Made a new limelight system with limeliteHelpers --- src/main/java/frc/robot/subsystems/limelite/Vision.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/robot/subsystems/limelite/Vision.java b/src/main/java/frc/robot/subsystems/limelite/Vision.java index 03f8d74..164c7db 100644 --- a/src/main/java/frc/robot/subsystems/limelite/Vision.java +++ b/src/main/java/frc/robot/subsystems/limelite/Vision.java @@ -33,3 +33,4 @@ public static void main(String[] args) { } } } +// \ No newline at end of file From 5c5191f99ea6e0999e5ed281d86ab7705052dd38 Mon Sep 17 00:00:00 2001 From: Adharrsh Date: Sat, 25 Jan 2025 16:52:13 -0500 Subject: [PATCH 7/7] Made a new limelight system with limeliteHelpers --- src/main/java/frc/robot/RobotContainer.java | 26 --------- .../subsystems/limelite/LimelightHelpers.java | 4 -- .../frc/robot/subsystems/limelite/Vision.java | 57 ++++++++++--------- 3 files changed, 31 insertions(+), 56 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 26c2c6a..24dc0fe 100755 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,7 +11,6 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.*; import frc.robot.constants.Constants; -import frc.robot.subsystems.limelite.vision; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import java.util.function.BooleanSupplier; @@ -46,9 +45,7 @@ public RobotContainer() { * joysticks}. */ private void configureBindings() { - new JoystickButton(driverJoystick, XboxController.Button.kA.value).onTrue(new InstantCommand(this::updateLimeLightStatus)); - new JoystickButton(driverJoystick, XboxController.Button.kB.value).onTrue(new InstantCommand(this::adjustPositionOfCrosshair)); } /** @@ -60,29 +57,6 @@ public Command getAutonomousCommand() { return null; } - private void updateLimeLightStatus(){ - boolean isTargetDetected = vision.isTarget(); - vision.updateLedMode(isTargetDetected); - } - - private void adjustPositionOfCrosshair(){ - double tx = vision.getTx(); - double ty = vision.getTy(); - - if (Math.abs(tx) > 1.00){ - System.out.println("Crosshair IS NOT aligned horizontally by [ " + tx + " ]"); - } - else{ - System.out.println("Crosshair IS aligned horizontally by [ " + tx + " ]"); - } - if (Math.abs(ty) > 1.00){ - System.out.println("Crosshair IS NOT aligned vertically by [ " + ty + " ]"); - } - else{ - System.out.println("Crosshair IS aligned horizontally by [ " + ty + " ]"); - } - - } } diff --git a/src/main/java/frc/robot/subsystems/limelite/LimelightHelpers.java b/src/main/java/frc/robot/subsystems/limelite/LimelightHelpers.java index afb26bc..2ddc2ac 100644 --- a/src/main/java/frc/robot/subsystems/limelite/LimelightHelpers.java +++ b/src/main/java/frc/robot/subsystems/limelite/LimelightHelpers.java @@ -462,10 +462,6 @@ public LimelightResults() { targets_Barcode = new LimelightTarget_Barcode[0]; } - - - public Joystick getBestTarget() { - } } /** diff --git a/src/main/java/frc/robot/subsystems/limelite/Vision.java b/src/main/java/frc/robot/subsystems/limelite/Vision.java index 164c7db..c4079b9 100644 --- a/src/main/java/frc/robot/subsystems/limelite/Vision.java +++ b/src/main/java/frc/robot/subsystems/limelite/Vision.java @@ -1,36 +1,41 @@ package frc.robot.subsystems.limelite; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.subsystems.limelite.LimelightHelpers; -import frc.robot.subsystems.limelite.LimelightHelpers.LimelightResults; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; +import java.util.concurrent.Executors; +import java.util.concurrent.ScheduledExecutorService; +import java.util.concurrent.TimeUnit; -public class Vision { - public static void main(String[] args) { - // Fetch the latest results from the Limelight - LimelightResults results = LimelightHelpers.getLatestResults("limelight"); - // Check the number of detected April Tags - int numAprilTags = results.targets_Fiducials.length; - System.out.println("Number of April Tags detected: " + numAprilTags); +public class Vision extends SubsystemBase { + private final String limelightName = ""; + ScheduledExecutorService scheduler = Executors.newScheduledThreadPool(1); + public Vision() { + LimelightHelpers.SetRobotOrientation(limelightName, 12, 12, 0, 0, 0, 0); + LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelightName); + LimelightHelpers.LimelightResults results = LimelightHelpers.getLatestResults(limelightName); + LimelightHelpers.LimelightTarget_Fiducial[] fiducials = results.targets_Fiducials; + scheduler.scheduleAtFixedRate(this::periodic, 0, 10, TimeUnit.SECONDS); - if (numAprilTags > 0) { - // Assuming we are interested in the first detected April Tag - LimelightHelpers.LimelightTarget_Fiducial target = results.targets_Fiducials[0]; + } - // Extract and print the pose and other relevant data - System.out.println("April Tag ID: " + target.fiducialID); - System.out.println("Pose (Field Space): " + target.getRobotPose_FieldSpace2D()); - System.out.println("Target Area: " + target.ta); - System.out.println("Horizontal Offset: " + target.tx); - System.out.println("Vertical Offset: " + target.ty); + @Override + public void periodic() { + LimelightHelpers.LimelightResults results = LimelightHelpers.getLatestResults(limelightName); + LimelightHelpers.LimelightTarget_Fiducial[] fiducials = results.targets_Fiducials; + if (fiducials != null && fiducials.length > 0) { + for (LimelightHelpers.LimelightTarget_Fiducial fiducial : fiducials) { + System.out.println("Fiducial ID: " + fiducial.fiducialID); + System.out.println("Fiducial Area: " + fiducial.ta); + System.out.println("Camera Pose (Target Space): " + fiducial.getCameraPose_TargetSpace2D()); + System.out.println("Robot Pose (Field Space): " + fiducial.getRobotPose_FieldSpace2D()); + System.out.println("Robot Pose (Target Space): " + fiducial.getRobotPose_TargetSpace2D()); + System.out.println("Target Pose (Camera Space): " + fiducial.getTargetPose_CameraSpace2D()); + System.out.println("Target Pose (Robot Space): " + fiducial.getTargetPose_RobotSpace2D()); + System.out.println("--------------------------"); + } } else { - System.out.println("No April Tags detected."); + System.out.println("No fiducial targets detected."); } + } -} -// \ No newline at end of file +} \ No newline at end of file