From 711167cfb202c08a1a2e5b6cdf669bd2c1c7f9ca Mon Sep 17 00:00:00 2001 From: rafaelbaird <155581003+rafaelbaird@users.noreply.github.com> Date: Mon, 12 Jan 2026 16:21:54 -0500 Subject: [PATCH 1/9] Created limelight.java --- .../robot/subsystems/vision/Limelight.java | 418 ++++++++++++++++++ 1 file changed, 418 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/vision/Limelight.java diff --git a/src/main/java/frc/robot/subsystems/vision/Limelight.java b/src/main/java/frc/robot/subsystems/vision/Limelight.java new file mode 100644 index 00000000..722f1905 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/Limelight.java @@ -0,0 +1,418 @@ +package frc.robot.vision; + +import java.util.HashMap; +import java.util.Optional; +import java.util.function.DoubleSupplier; + +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +import com.ctre.phoenix6.Utils; + +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.math.filter.Debouncer.DebounceType; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.networktables.DoublePublisher; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Robot; +import frc.robot.RobotContainer; +import frc.robot.util.PoseUtils; +import frc.robot.vision.LimelightHelpers.RawFiducial; + +public class Limelight extends SubsystemBase { + + /* CONSTANTS */ + public static final double coralStationTagHeightMeters = 1.35255; // make sure these two are correct + // does it need to be to the center of the tag? + public static final double reefTagHeightMeters = // 0.174625; + 0.3; + public static final double reefOffsetFromCenterOfTag = 0; + + public static final int[] reefIDsRed = { 6, 7, 8, 9, 10, 11 }; + public static final int[] reefIDsBlue = { 17, 18, 19, 20, 21, 22 }; + public static final int[] reefIDs = { 6, 7, 8, 9, 10, 11, 17, 18, 19, 20, 21, 22 }; + + public static final int[] coralStationIDsRed = { 1, 2 }; + public static final int[] coralStationIDsBlue = { 12, 13 }; + public static final int[] coralStationIDs = { 1, 2, 12, 13 }; + public static HashMap tagRotationsMap = new HashMap(); + { + tagRotationsMap.put(6, Rotation2d.fromDegrees(120)); + tagRotationsMap.put(7, Rotation2d.fromDegrees(180)); + tagRotationsMap.put(8, Rotation2d.fromDegrees(-120)); + tagRotationsMap.put(9, Rotation2d.fromDegrees(-60)); + tagRotationsMap.put(10, Rotation2d.fromDegrees(0)); + tagRotationsMap.put(11, Rotation2d.fromDegrees(60)); + + // TODO: Should these be flipped? + tagRotationsMap.put(17, Rotation2d.fromDegrees(60)); + tagRotationsMap.put(18, Rotation2d.fromDegrees(0)); + tagRotationsMap.put(19, Rotation2d.fromDegrees(-60)); + tagRotationsMap.put(20, Rotation2d.fromDegrees(-120)); + tagRotationsMap.put(21, Rotation2d.fromDegrees(180)); + tagRotationsMap.put(22, Rotation2d.fromDegrees(120)); + } + public static final double TARGET_DEBOUNCE_TIME = 0.2; + + /* INSTANCE VARIABLES */ + private int tagCount; + private int[] validIDs = {}; // TODO: set these + public String cameraName; + private double tx; + private double ty; + private Debouncer targetDebouncer = new Debouncer(TARGET_DEBOUNCE_TIME, DebounceType.kFalling); + + public static final double angleVelocityTolerance = 360 * Math.PI / 180; // in radians per sec + + private double cameraHeightMeters; + public double cameraAngle; + public double cameraOffsetX; // right is positive + public double cameraOffsetY; // forward is positive + private double angleMult; + + private boolean hasTipped; + + private DoublePublisher yDistPub; + private DoublePublisher xDistPub; + private DoublePublisher horizontalDistPub; + + // TODO setup camera IPs? + // https://docs.limelightvision.io/docs/docs-limelight/getting-started/FRC/best-practices + public Limelight(String cameraName, double cameraHeightMeters, double cameraAngle, double cameraOffsetX, + double cameraOffsetY, boolean cameraUpsideDown) { + this.cameraName = cameraName; + this.cameraHeightMeters = cameraHeightMeters; + this.cameraAngle = cameraAngle; + this.cameraOffsetX = cameraOffsetX; + this.cameraOffsetY = cameraOffsetY; + LimelightHelpers.SetFiducialIDFiltersOverride(cameraName, validIDs); + if (cameraUpsideDown) { + angleMult = -1; + } else { + angleMult = 1; + } + + NetworkTableInstance inst = NetworkTableInstance.getDefault(); + NetworkTable lightTable = inst.getTable(cameraName); + + yDistPub = lightTable.getDoubleTopic("Y Distance").publish(); + xDistPub = lightTable.getDoubleTopic("X Distance").publish(); + horizontalDistPub = lightTable.getDoubleTopic("Horizontal Distance").publish(); + } + + // might not be needed + public static boolean isCorrectID(int ID, int... IDs) { + for (int n : IDs) { + if (n == ID) + return true; + } + return false; + } + + // from last years robot + public double getTimestampSeconds() { + double latency = (LimelightHelpers.getLimelightNTDouble(cameraName, "cl") + + LimelightHelpers.getLimelightNTDouble(cameraName, "tl")) + / 1000.0; + + return Timer.getFPGATimestamp() - latency; + } + + // from last years robot as well + public boolean hasValidTarget() { + boolean hasMatch = (LimelightHelpers.getLimelightNTDouble(cameraName, "tv") == 1.0); + return targetDebouncer.calculate(hasMatch); + } + + public void disable() { + // https://docs.limelightvision.io/docs/docs-limelight/pipeline-apriltag/apriltag-robot-localization-megatag2#using-limelight-4s-built-in-imu-with-imumode_set--setimumode + // https://docs.limelightvision.io/docs/docs-limelight/software-change-log#limelight-os-20251-final-release---22425-test-release---21825 + LimelightHelpers.SetIMUMode(cameraName, 1); // If not moving reset internal IMU + // LimelightHelpers.setLimelightNTDouble(cameraName, "throttle_set", 200); // + // manage thermals + } + + public void setGyroMode(int mode) { + LimelightHelpers.SetIMUMode(cameraName, mode); + } + + public void enable() { + LimelightHelpers.SetIMUMode(cameraName, 1); // if moving use builtin, maybe change to 4 + // LimelightHelpers.setLimelightNTDouble(cameraName, "throttle_set", 0); //TODO + // check needs to be 1? // manage thermals + } + + public RawFiducial getClosestTag() { + RawFiducial[] tags = LimelightHelpers.getRawFiducials(cameraName); + if (tags.length == 0) { + return null; + } + RawFiducial largest = tags[0]; + for (RawFiducial tag : tags) { + if (tag.distToRobot > largest.distToRobot) { + largest = tag; + } + } + return largest; + } + + public Rotation2d getClosestTagAngle() { + int closestId = getClosestTag().id; + return tagRotationsMap.get(closestId); + } + + public void poseEstimationMegatag2() { + + double angle = (RobotContainer.drivetrain.getWrappedHeading().getDegrees() + 360) % 360; + LimelightHelpers.SetRobotOrientation(cameraName, angle, 0, 0, 0, 0, 0); + LimelightHelpers.PoseEstimate mt2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(cameraName); + + boolean overrideReject = false; + boolean isTipping = Math.abs(RobotContainer.drivetrain.getPigeon2().getPitch().getValueAsDouble()) > 2 + || Math.abs(RobotContainer.drivetrain.getPigeon2().getRoll().getValueAsDouble()) > 2; + Logger.recordOutput(cameraName + "/isTipping", isTipping); + + if (hasTipped && !isTipping) { + overrideReject = true; + } + + boolean shouldRejectUpdate = false; + int rejectReason = 0; + if (mt2 != null) { + Optional optPastRobotPose = RobotContainer.drivetrain.getPoseAtTime(mt2.timestampSeconds); + if (optPastRobotPose.isPresent()) { + Logger.recordOutput(cameraName + "/PastRobotPose", optPastRobotPose.get()); + } + Pose2d pastRobotPose = RobotContainer.drivetrain.getRobotPose(); + // Pose2d pastRobotPose = optPastRobotPose.orElseGet(() -> RobotContainer.drivetrain.getRobotPose()); + Logger.recordOutput(cameraName + "/timestampSeconds", mt2.timestampSeconds); + RawFiducial[] tags = mt2.rawFiducials; + int[] ids = new int[tags.length]; + for (int i = 0; i < tags.length; i++) { + ids[i] = tags[i].id; + } + Logger.recordOutput(cameraName + "/SeenTags", ids); + Logger.recordOutput(cameraName + "/PoseLatency", mt2.timestampSeconds - Timer.getFPGATimestamp()); + if (mt2.tagCount == 0) { + // rejects current measurement if there are no aprilTags + shouldRejectUpdate = true; + rejectReason = 1; + } + if (Math.abs(RobotContainer.drivetrain.getCurrentSpeeds().omegaRadiansPerSecond) > angleVelocityTolerance) { + shouldRejectUpdate = true; + rejectReason = 2; + } + if ((mt2.pose.getTranslation().getDistance(pastRobotPose.getTranslation()) > 0.9 + && !DriverStation.isDisabled() && !DriverStation.isTeleopEnabled())) { + shouldRejectUpdate = true; + rejectReason = 3; + } + if (Math.abs(PoseUtils.wrapRotation(mt2.pose.getRotation()) + .minus(PoseUtils.wrapRotation(pastRobotPose.getRotation())).getDegrees()) > 3) { + shouldRejectUpdate = true; + rejectReason = 4; + } + if (mt2.avgTagDist > 4) { + shouldRejectUpdate = true; + rejectReason = 5; + } + // if (isTipping) { + // shouldRejectUpdate = true; + // } + // adds vision measurement if conditions are met + if (!shouldRejectUpdate) { + Logger.recordOutput(cameraName + "/mt2Pose", mt2.pose); + Logger.recordOutput(cameraName + "/Calculated stdevs", + Math.pow(0.5, mt2.tagCount) * 2 * mt2.avgTagDist); + // Vector = VecBuilder.fill + RobotContainer.drivetrain.addVisionMeasurement( + mt2.pose, + Utils.fpgaToCurrentTime(mt2.timestampSeconds), + // VecBuilder.fill(0.000716, 0.0003, Double.POSITIVE_INFINITY)); + VecBuilder.fill(Math.pow(0.5, mt2.tagCount) * 2 * mt2.avgTagDist, + Math.pow(0.5, mt2.tagCount) * 2 * mt2.avgTagDist, Double.POSITIVE_INFINITY)); + if (DriverStation.isTeleopEnabled()) { + RobotContainer.leds.playLEDPattern(LEDs.holding(Color.kWhite), 0.2); + } + } else { + Logger.recordOutput(cameraName + "/mt2PoseRejected", mt2.pose); + Logger.recordOutput(cameraName + "/rejectReason", rejectReason); + } + } + } + + // TODO: Do we need these / check if the trig is right + + public double getDistanceToTag(double tagHeightMeters) { + if (hasValidTarget()) { + double distance = getStraightDistanceToTag(tagHeightMeters) - cameraOffsetY; + return distance / Math.cos((Math.PI / 180.0) * getTX()); + } + return 0; + } + + public double getStraightDistanceToTag(double tagHeightMeters) { + if (hasValidTarget()) { + double distance = (tagHeightMeters - cameraHeightMeters) + / Math.tan( + (Math.PI / 180.0) + * (cameraAngle + getTY())); + return distance + cameraOffsetY; + } + return 0; + } + + public double getHorizontalDistanceToTag(double tagHeightMeters) { + if (hasValidTarget()) { + double distance = getStraightDistanceToTag(tagHeightMeters) - cameraOffsetY; + + distance = distance * Math.tan(getTX() * (Math.PI / 180.0)); + return distance + cameraOffsetX; + } + return 0; + } + + public double getDistanceToCoralStation() { + return getDistanceToTag(coralStationTagHeightMeters); + } + + public double getStraightDistanceToCoralStation() { + return getStraightDistanceToTag(coralStationTagHeightMeters); + + } + + public double getHorizontalDistanceToCoralStation() { + return getHorizontalDistanceToTag(coralStationTagHeightMeters); + } + + // ISN'T OFFSET FOR THE CENTER OF THE ROBOT!!!!!!! + public double getDistanceToReef() { + return getDistanceToTag(reefTagHeightMeters); + } + + // TODO: Do we need these / check if the trig is right + public double getStraightDistanceToReef() { + return getStraightDistanceToTag(reefTagHeightMeters); + } + + public double getHorizontalDistanceToReef() { + return getHorizontalDistanceToTag(reefTagHeightMeters); + } + + @AutoLogOutput + public double getTX() { + return tx * angleMult; + } + + @AutoLogOutput + public double getTY() { + return ty * -angleMult; + } + + public DoubleSupplier tySupplier() { + return () -> getTY(); + } + + public DoubleSupplier txSupplier() { + return () -> getTX(); + } + + // TODO: Do we need these / check if the trig is right + // public double getStraightDistanceToTag() { + // if (hasValidTarget()) + // return goalHeightReef / (Math.tan(Math.toRadians(getTY() + + // limelightOffsetAngleVertical))); + // return 0; + // } + + // TODO: Do we need these / check if the trig is right + public double getStrafeDistanceToReef() { + if (isCorrectID(getTagID(), reefIDs)) { + return (Math.tan(Math.toRadians(getTX()))) * getStraightDistanceToReef(); + } + return 0; + } + + public int getTagID() { + return (int) LimelightHelpers.getFiducialID(cameraName); + } + + public void periodic() { + + if (Math.abs(RobotContainer.drivetrain.getPigeon2().getPitch().getValueAsDouble()) > 0.3 + || Math.abs(RobotContainer.drivetrain.getPigeon2().getRoll().getValueAsDouble()) > 0.3) { + hasTipped = true; + } + // tagID = (int) Limetable.getEntry("tid").getDouble(-1); + // TODO if you get a pose estimate in the frame before this is applied it may + // not work + tx = LimelightHelpers.getTX(cameraName); + ty = LimelightHelpers.getTY(cameraName); + RawFiducial[] allTags = LimelightHelpers.getRawFiducials(cameraName); + int numValidTags = 0; + for (LimelightHelpers.RawFiducial t : allTags) { + if (t.distToCamera < 4.0) { + numValidTags++; + } + } + + int[] validTags = new int[numValidTags]; + int counter = 0; + for (RawFiducial t : allTags) { + if (t.distToCamera < 4.0) { + validTags[counter] = t.id; + counter++; + } + } + // LimelightHelpers.SetFiducialIDFiltersOverride(cameraName, validTags); + poseEstimationMegatag2(); + xDistPub.set(getHorizontalDistanceToReef()); + yDistPub.set(getStraightDistanceToReef()); + horizontalDistPub.set(getDistanceToReef()); + + double[] poseArr = LimelightHelpers.getBotPose_TargetSpace(cameraName); + Pose2d botPose = new Pose2d(); + if (poseArr.length >= 6) { + botPose = new Pose2d(poseArr[0], poseArr[2], Rotation2d.fromDegrees(poseArr[4])); + } + Logger.recordOutput(cameraName + "/IMUYaw", + LimelightHelpers.getIMUData(cameraName).robotYaw * (Math.PI / 180.0)); // TODO should be yaw? + Logger.recordOutput(cameraName + "/BotPoseTargetSpace", botPose); + Logger.recordOutput(cameraName + "/BotPose3dTargetSpace", + LimelightHelpers.getBotPose3d_TargetSpace(cameraName)); + + var entry = LimelightHelpers.getLimelightNTTableEntry(cameraName, "tcornxy"); + if (entry != null) { + var tcornxy = entry.getDoubleArray(new double[0]); + if (tcornxy != null && tcornxy.length > 0) { + Logger.recordOutput(cameraName + "/tcornxy", tcornxy); + } + } + } + + public Command flashLEDs() { + return Commands.sequence( + Commands.runOnce(() -> LimelightHelpers.setLEDMode_ForceBlink(cameraName)), + Commands.waitSeconds(0.6), + Commands.runOnce(() -> LimelightHelpers.setLEDMode_ForceOff(cameraName)) + ); + } + + + + public Command ifHasTarget(Command cmd) { + return cmd.onlyWhile(this::hasValidTarget); + } +} + From d99f608a15c7893f5fa28f8b44ca1e8946d27641 Mon Sep 17 00:00:00 2001 From: rafaelbaird <155581003+rafaelbaird@users.noreply.github.com> Date: Mon, 12 Jan 2026 16:22:59 -0500 Subject: [PATCH 2/9] Created limelight.java --- .../robot/subsystems/vision/Limelight.java | 418 ++++++++++++++++++ 1 file changed, 418 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/vision/Limelight.java diff --git a/src/main/java/frc/robot/subsystems/vision/Limelight.java b/src/main/java/frc/robot/subsystems/vision/Limelight.java new file mode 100644 index 00000000..722f1905 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/Limelight.java @@ -0,0 +1,418 @@ +package frc.robot.vision; + +import java.util.HashMap; +import java.util.Optional; +import java.util.function.DoubleSupplier; + +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +import com.ctre.phoenix6.Utils; + +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.math.filter.Debouncer.DebounceType; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.networktables.DoublePublisher; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Robot; +import frc.robot.RobotContainer; +import frc.robot.util.PoseUtils; +import frc.robot.vision.LimelightHelpers.RawFiducial; + +public class Limelight extends SubsystemBase { + + /* CONSTANTS */ + public static final double coralStationTagHeightMeters = 1.35255; // make sure these two are correct + // does it need to be to the center of the tag? + public static final double reefTagHeightMeters = // 0.174625; + 0.3; + public static final double reefOffsetFromCenterOfTag = 0; + + public static final int[] reefIDsRed = { 6, 7, 8, 9, 10, 11 }; + public static final int[] reefIDsBlue = { 17, 18, 19, 20, 21, 22 }; + public static final int[] reefIDs = { 6, 7, 8, 9, 10, 11, 17, 18, 19, 20, 21, 22 }; + + public static final int[] coralStationIDsRed = { 1, 2 }; + public static final int[] coralStationIDsBlue = { 12, 13 }; + public static final int[] coralStationIDs = { 1, 2, 12, 13 }; + public static HashMap tagRotationsMap = new HashMap(); + { + tagRotationsMap.put(6, Rotation2d.fromDegrees(120)); + tagRotationsMap.put(7, Rotation2d.fromDegrees(180)); + tagRotationsMap.put(8, Rotation2d.fromDegrees(-120)); + tagRotationsMap.put(9, Rotation2d.fromDegrees(-60)); + tagRotationsMap.put(10, Rotation2d.fromDegrees(0)); + tagRotationsMap.put(11, Rotation2d.fromDegrees(60)); + + // TODO: Should these be flipped? + tagRotationsMap.put(17, Rotation2d.fromDegrees(60)); + tagRotationsMap.put(18, Rotation2d.fromDegrees(0)); + tagRotationsMap.put(19, Rotation2d.fromDegrees(-60)); + tagRotationsMap.put(20, Rotation2d.fromDegrees(-120)); + tagRotationsMap.put(21, Rotation2d.fromDegrees(180)); + tagRotationsMap.put(22, Rotation2d.fromDegrees(120)); + } + public static final double TARGET_DEBOUNCE_TIME = 0.2; + + /* INSTANCE VARIABLES */ + private int tagCount; + private int[] validIDs = {}; // TODO: set these + public String cameraName; + private double tx; + private double ty; + private Debouncer targetDebouncer = new Debouncer(TARGET_DEBOUNCE_TIME, DebounceType.kFalling); + + public static final double angleVelocityTolerance = 360 * Math.PI / 180; // in radians per sec + + private double cameraHeightMeters; + public double cameraAngle; + public double cameraOffsetX; // right is positive + public double cameraOffsetY; // forward is positive + private double angleMult; + + private boolean hasTipped; + + private DoublePublisher yDistPub; + private DoublePublisher xDistPub; + private DoublePublisher horizontalDistPub; + + // TODO setup camera IPs? + // https://docs.limelightvision.io/docs/docs-limelight/getting-started/FRC/best-practices + public Limelight(String cameraName, double cameraHeightMeters, double cameraAngle, double cameraOffsetX, + double cameraOffsetY, boolean cameraUpsideDown) { + this.cameraName = cameraName; + this.cameraHeightMeters = cameraHeightMeters; + this.cameraAngle = cameraAngle; + this.cameraOffsetX = cameraOffsetX; + this.cameraOffsetY = cameraOffsetY; + LimelightHelpers.SetFiducialIDFiltersOverride(cameraName, validIDs); + if (cameraUpsideDown) { + angleMult = -1; + } else { + angleMult = 1; + } + + NetworkTableInstance inst = NetworkTableInstance.getDefault(); + NetworkTable lightTable = inst.getTable(cameraName); + + yDistPub = lightTable.getDoubleTopic("Y Distance").publish(); + xDistPub = lightTable.getDoubleTopic("X Distance").publish(); + horizontalDistPub = lightTable.getDoubleTopic("Horizontal Distance").publish(); + } + + // might not be needed + public static boolean isCorrectID(int ID, int... IDs) { + for (int n : IDs) { + if (n == ID) + return true; + } + return false; + } + + // from last years robot + public double getTimestampSeconds() { + double latency = (LimelightHelpers.getLimelightNTDouble(cameraName, "cl") + + LimelightHelpers.getLimelightNTDouble(cameraName, "tl")) + / 1000.0; + + return Timer.getFPGATimestamp() - latency; + } + + // from last years robot as well + public boolean hasValidTarget() { + boolean hasMatch = (LimelightHelpers.getLimelightNTDouble(cameraName, "tv") == 1.0); + return targetDebouncer.calculate(hasMatch); + } + + public void disable() { + // https://docs.limelightvision.io/docs/docs-limelight/pipeline-apriltag/apriltag-robot-localization-megatag2#using-limelight-4s-built-in-imu-with-imumode_set--setimumode + // https://docs.limelightvision.io/docs/docs-limelight/software-change-log#limelight-os-20251-final-release---22425-test-release---21825 + LimelightHelpers.SetIMUMode(cameraName, 1); // If not moving reset internal IMU + // LimelightHelpers.setLimelightNTDouble(cameraName, "throttle_set", 200); // + // manage thermals + } + + public void setGyroMode(int mode) { + LimelightHelpers.SetIMUMode(cameraName, mode); + } + + public void enable() { + LimelightHelpers.SetIMUMode(cameraName, 1); // if moving use builtin, maybe change to 4 + // LimelightHelpers.setLimelightNTDouble(cameraName, "throttle_set", 0); //TODO + // check needs to be 1? // manage thermals + } + + public RawFiducial getClosestTag() { + RawFiducial[] tags = LimelightHelpers.getRawFiducials(cameraName); + if (tags.length == 0) { + return null; + } + RawFiducial largest = tags[0]; + for (RawFiducial tag : tags) { + if (tag.distToRobot > largest.distToRobot) { + largest = tag; + } + } + return largest; + } + + public Rotation2d getClosestTagAngle() { + int closestId = getClosestTag().id; + return tagRotationsMap.get(closestId); + } + + public void poseEstimationMegatag2() { + + double angle = (RobotContainer.drivetrain.getWrappedHeading().getDegrees() + 360) % 360; + LimelightHelpers.SetRobotOrientation(cameraName, angle, 0, 0, 0, 0, 0); + LimelightHelpers.PoseEstimate mt2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(cameraName); + + boolean overrideReject = false; + boolean isTipping = Math.abs(RobotContainer.drivetrain.getPigeon2().getPitch().getValueAsDouble()) > 2 + || Math.abs(RobotContainer.drivetrain.getPigeon2().getRoll().getValueAsDouble()) > 2; + Logger.recordOutput(cameraName + "/isTipping", isTipping); + + if (hasTipped && !isTipping) { + overrideReject = true; + } + + boolean shouldRejectUpdate = false; + int rejectReason = 0; + if (mt2 != null) { + Optional optPastRobotPose = RobotContainer.drivetrain.getPoseAtTime(mt2.timestampSeconds); + if (optPastRobotPose.isPresent()) { + Logger.recordOutput(cameraName + "/PastRobotPose", optPastRobotPose.get()); + } + Pose2d pastRobotPose = RobotContainer.drivetrain.getRobotPose(); + // Pose2d pastRobotPose = optPastRobotPose.orElseGet(() -> RobotContainer.drivetrain.getRobotPose()); + Logger.recordOutput(cameraName + "/timestampSeconds", mt2.timestampSeconds); + RawFiducial[] tags = mt2.rawFiducials; + int[] ids = new int[tags.length]; + for (int i = 0; i < tags.length; i++) { + ids[i] = tags[i].id; + } + Logger.recordOutput(cameraName + "/SeenTags", ids); + Logger.recordOutput(cameraName + "/PoseLatency", mt2.timestampSeconds - Timer.getFPGATimestamp()); + if (mt2.tagCount == 0) { + // rejects current measurement if there are no aprilTags + shouldRejectUpdate = true; + rejectReason = 1; + } + if (Math.abs(RobotContainer.drivetrain.getCurrentSpeeds().omegaRadiansPerSecond) > angleVelocityTolerance) { + shouldRejectUpdate = true; + rejectReason = 2; + } + if ((mt2.pose.getTranslation().getDistance(pastRobotPose.getTranslation()) > 0.9 + && !DriverStation.isDisabled() && !DriverStation.isTeleopEnabled())) { + shouldRejectUpdate = true; + rejectReason = 3; + } + if (Math.abs(PoseUtils.wrapRotation(mt2.pose.getRotation()) + .minus(PoseUtils.wrapRotation(pastRobotPose.getRotation())).getDegrees()) > 3) { + shouldRejectUpdate = true; + rejectReason = 4; + } + if (mt2.avgTagDist > 4) { + shouldRejectUpdate = true; + rejectReason = 5; + } + // if (isTipping) { + // shouldRejectUpdate = true; + // } + // adds vision measurement if conditions are met + if (!shouldRejectUpdate) { + Logger.recordOutput(cameraName + "/mt2Pose", mt2.pose); + Logger.recordOutput(cameraName + "/Calculated stdevs", + Math.pow(0.5, mt2.tagCount) * 2 * mt2.avgTagDist); + // Vector = VecBuilder.fill + RobotContainer.drivetrain.addVisionMeasurement( + mt2.pose, + Utils.fpgaToCurrentTime(mt2.timestampSeconds), + // VecBuilder.fill(0.000716, 0.0003, Double.POSITIVE_INFINITY)); + VecBuilder.fill(Math.pow(0.5, mt2.tagCount) * 2 * mt2.avgTagDist, + Math.pow(0.5, mt2.tagCount) * 2 * mt2.avgTagDist, Double.POSITIVE_INFINITY)); + if (DriverStation.isTeleopEnabled()) { + RobotContainer.leds.playLEDPattern(LEDs.holding(Color.kWhite), 0.2); + } + } else { + Logger.recordOutput(cameraName + "/mt2PoseRejected", mt2.pose); + Logger.recordOutput(cameraName + "/rejectReason", rejectReason); + } + } + } + + // TODO: Do we need these / check if the trig is right + + public double getDistanceToTag(double tagHeightMeters) { + if (hasValidTarget()) { + double distance = getStraightDistanceToTag(tagHeightMeters) - cameraOffsetY; + return distance / Math.cos((Math.PI / 180.0) * getTX()); + } + return 0; + } + + public double getStraightDistanceToTag(double tagHeightMeters) { + if (hasValidTarget()) { + double distance = (tagHeightMeters - cameraHeightMeters) + / Math.tan( + (Math.PI / 180.0) + * (cameraAngle + getTY())); + return distance + cameraOffsetY; + } + return 0; + } + + public double getHorizontalDistanceToTag(double tagHeightMeters) { + if (hasValidTarget()) { + double distance = getStraightDistanceToTag(tagHeightMeters) - cameraOffsetY; + + distance = distance * Math.tan(getTX() * (Math.PI / 180.0)); + return distance + cameraOffsetX; + } + return 0; + } + + public double getDistanceToCoralStation() { + return getDistanceToTag(coralStationTagHeightMeters); + } + + public double getStraightDistanceToCoralStation() { + return getStraightDistanceToTag(coralStationTagHeightMeters); + + } + + public double getHorizontalDistanceToCoralStation() { + return getHorizontalDistanceToTag(coralStationTagHeightMeters); + } + + // ISN'T OFFSET FOR THE CENTER OF THE ROBOT!!!!!!! + public double getDistanceToReef() { + return getDistanceToTag(reefTagHeightMeters); + } + + // TODO: Do we need these / check if the trig is right + public double getStraightDistanceToReef() { + return getStraightDistanceToTag(reefTagHeightMeters); + } + + public double getHorizontalDistanceToReef() { + return getHorizontalDistanceToTag(reefTagHeightMeters); + } + + @AutoLogOutput + public double getTX() { + return tx * angleMult; + } + + @AutoLogOutput + public double getTY() { + return ty * -angleMult; + } + + public DoubleSupplier tySupplier() { + return () -> getTY(); + } + + public DoubleSupplier txSupplier() { + return () -> getTX(); + } + + // TODO: Do we need these / check if the trig is right + // public double getStraightDistanceToTag() { + // if (hasValidTarget()) + // return goalHeightReef / (Math.tan(Math.toRadians(getTY() + + // limelightOffsetAngleVertical))); + // return 0; + // } + + // TODO: Do we need these / check if the trig is right + public double getStrafeDistanceToReef() { + if (isCorrectID(getTagID(), reefIDs)) { + return (Math.tan(Math.toRadians(getTX()))) * getStraightDistanceToReef(); + } + return 0; + } + + public int getTagID() { + return (int) LimelightHelpers.getFiducialID(cameraName); + } + + public void periodic() { + + if (Math.abs(RobotContainer.drivetrain.getPigeon2().getPitch().getValueAsDouble()) > 0.3 + || Math.abs(RobotContainer.drivetrain.getPigeon2().getRoll().getValueAsDouble()) > 0.3) { + hasTipped = true; + } + // tagID = (int) Limetable.getEntry("tid").getDouble(-1); + // TODO if you get a pose estimate in the frame before this is applied it may + // not work + tx = LimelightHelpers.getTX(cameraName); + ty = LimelightHelpers.getTY(cameraName); + RawFiducial[] allTags = LimelightHelpers.getRawFiducials(cameraName); + int numValidTags = 0; + for (LimelightHelpers.RawFiducial t : allTags) { + if (t.distToCamera < 4.0) { + numValidTags++; + } + } + + int[] validTags = new int[numValidTags]; + int counter = 0; + for (RawFiducial t : allTags) { + if (t.distToCamera < 4.0) { + validTags[counter] = t.id; + counter++; + } + } + // LimelightHelpers.SetFiducialIDFiltersOverride(cameraName, validTags); + poseEstimationMegatag2(); + xDistPub.set(getHorizontalDistanceToReef()); + yDistPub.set(getStraightDistanceToReef()); + horizontalDistPub.set(getDistanceToReef()); + + double[] poseArr = LimelightHelpers.getBotPose_TargetSpace(cameraName); + Pose2d botPose = new Pose2d(); + if (poseArr.length >= 6) { + botPose = new Pose2d(poseArr[0], poseArr[2], Rotation2d.fromDegrees(poseArr[4])); + } + Logger.recordOutput(cameraName + "/IMUYaw", + LimelightHelpers.getIMUData(cameraName).robotYaw * (Math.PI / 180.0)); // TODO should be yaw? + Logger.recordOutput(cameraName + "/BotPoseTargetSpace", botPose); + Logger.recordOutput(cameraName + "/BotPose3dTargetSpace", + LimelightHelpers.getBotPose3d_TargetSpace(cameraName)); + + var entry = LimelightHelpers.getLimelightNTTableEntry(cameraName, "tcornxy"); + if (entry != null) { + var tcornxy = entry.getDoubleArray(new double[0]); + if (tcornxy != null && tcornxy.length > 0) { + Logger.recordOutput(cameraName + "/tcornxy", tcornxy); + } + } + } + + public Command flashLEDs() { + return Commands.sequence( + Commands.runOnce(() -> LimelightHelpers.setLEDMode_ForceBlink(cameraName)), + Commands.waitSeconds(0.6), + Commands.runOnce(() -> LimelightHelpers.setLEDMode_ForceOff(cameraName)) + ); + } + + + + public Command ifHasTarget(Command cmd) { + return cmd.onlyWhile(this::hasValidTarget); + } +} + From 61921ff64e3d4a4fe7cbc3a29b53a898eb64e6ee Mon Sep 17 00:00:00 2001 From: sub0dev <125705137+Mr-Pyro@users.noreply.github.com> Date: Tue, 13 Jan 2026 15:52:40 -0500 Subject: [PATCH 3/9] Vision Added Limelight, limelight helpers, and updated drivetrain. --- gradlew | 0 src/main/java/frc/robot/BuildConstants.java | 14 +- .../drivetrain/CommandSwerveDrivetrain.java | 12 +- .../robot/subsystems/vision/Limelight.java | 625 +++---- .../subsystems/vision/LimelightHelpers.java | 1647 +++++++++++++++++ 5 files changed, 1929 insertions(+), 369 deletions(-) mode change 100644 => 100755 gradlew create mode 100644 src/main/java/frc/robot/subsystems/vision/LimelightHelpers.java diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 280f15e3..c15a016e 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,13 +5,13 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Rebuilt"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = -1; - public static final String GIT_SHA = "UNKNOWN"; - public static final String GIT_DATE = "UNKNOWN"; - public static final String GIT_BRANCH = "UNKNOWN"; - public static final String BUILD_DATE = "2026-01-12 00:23:28 EST"; - public static final long BUILD_UNIX_TIME = 1768195408487L; - public static final int DIRTY = 129; + public static final int GIT_REVISION = 4; + public static final String GIT_SHA = "5257b345fb5c3a43fa51ec15bb28b16a2a6abab5"; + public static final String GIT_DATE = "2026-01-12 16:23:13 EST"; + public static final String GIT_BRANCH = "vision"; + public static final String BUILD_DATE = "2026-01-12 16:25:45 EST"; + public static final long BUILD_UNIX_TIME = 1768253145169L; + public static final int DIRTY = 1; private BuildConstants() {} } diff --git a/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java index 3ce1feb1..e431ab7b 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java @@ -22,6 +22,7 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; @@ -39,11 +40,15 @@ import frc.robot.util.PoseUtils; import frc.robot.util.TunerConstants; import frc.robot.util.TunerConstants.TunerSwerveDrivetrain; + +import java.util.Optional; import java.util.function.Supplier; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Subsystem { + public TimeInterpolatableBuffer poseBuffer = TimeInterpolatableBuffer.createBuffer(3); + private static final double kSimLoopPeriod = 0.002; // 2 ms private Notifier m_simNotifier = null; @@ -388,7 +393,12 @@ public Command alignToAngleFieldRelativeCommand(Rotation2d angle, boolean lockDr Commands.run(() -> alignToAngleFieldRelative(lockDrive), this) .until(() -> isRobotAtAngleSetPoint)); } - + public ChassisSpeeds getCurrentSpeeds() { + return this.getState().Speeds; + } + public Optional getPoseAtTime(double time) { + return poseBuffer.getSample(time); + } @Override public void periodic() { diff --git a/src/main/java/frc/robot/subsystems/vision/Limelight.java b/src/main/java/frc/robot/subsystems/vision/Limelight.java index 722f1905..95172e2f 100644 --- a/src/main/java/frc/robot/subsystems/vision/Limelight.java +++ b/src/main/java/frc/robot/subsystems/vision/Limelight.java @@ -1,418 +1,321 @@ -package frc.robot.vision; - -import java.util.HashMap; -import java.util.Optional; -import java.util.function.DoubleSupplier; - -import org.littletonrobotics.junction.AutoLogOutput; -import org.littletonrobotics.junction.Logger; +package frc.robot.subsystems.vision; import com.ctre.phoenix6.Utils; - import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.Vector; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.numbers.N3; import edu.wpi.first.networktables.DoublePublisher; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Robot; import frc.robot.RobotContainer; import frc.robot.util.PoseUtils; -import frc.robot.vision.LimelightHelpers.RawFiducial; +import frc.robot.subsystems.vision.LimelightHelpers.RawFiducial; +import java.util.HashMap; +import java.util.Optional; +import java.util.function.DoubleSupplier; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; public class Limelight extends SubsystemBase { - /* CONSTANTS */ - public static final double coralStationTagHeightMeters = 1.35255; // make sure these two are correct - // does it need to be to the center of the tag? - public static final double reefTagHeightMeters = // 0.174625; - 0.3; - public static final double reefOffsetFromCenterOfTag = 0; - - public static final int[] reefIDsRed = { 6, 7, 8, 9, 10, 11 }; - public static final int[] reefIDsBlue = { 17, 18, 19, 20, 21, 22 }; - public static final int[] reefIDs = { 6, 7, 8, 9, 10, 11, 17, 18, 19, 20, 21, 22 }; - - public static final int[] coralStationIDsRed = { 1, 2 }; - public static final int[] coralStationIDsBlue = { 12, 13 }; - public static final int[] coralStationIDs = { 1, 2, 12, 13 }; - public static HashMap tagRotationsMap = new HashMap(); - { - tagRotationsMap.put(6, Rotation2d.fromDegrees(120)); - tagRotationsMap.put(7, Rotation2d.fromDegrees(180)); - tagRotationsMap.put(8, Rotation2d.fromDegrees(-120)); - tagRotationsMap.put(9, Rotation2d.fromDegrees(-60)); - tagRotationsMap.put(10, Rotation2d.fromDegrees(0)); - tagRotationsMap.put(11, Rotation2d.fromDegrees(60)); - - // TODO: Should these be flipped? - tagRotationsMap.put(17, Rotation2d.fromDegrees(60)); - tagRotationsMap.put(18, Rotation2d.fromDegrees(0)); - tagRotationsMap.put(19, Rotation2d.fromDegrees(-60)); - tagRotationsMap.put(20, Rotation2d.fromDegrees(-120)); - tagRotationsMap.put(21, Rotation2d.fromDegrees(180)); - tagRotationsMap.put(22, Rotation2d.fromDegrees(120)); - } - public static final double TARGET_DEBOUNCE_TIME = 0.2; - - /* INSTANCE VARIABLES */ - private int tagCount; - private int[] validIDs = {}; // TODO: set these - public String cameraName; - private double tx; - private double ty; - private Debouncer targetDebouncer = new Debouncer(TARGET_DEBOUNCE_TIME, DebounceType.kFalling); - - public static final double angleVelocityTolerance = 360 * Math.PI / 180; // in radians per sec - - private double cameraHeightMeters; - public double cameraAngle; - public double cameraOffsetX; // right is positive - public double cameraOffsetY; // forward is positive - private double angleMult; - - private boolean hasTipped; - - private DoublePublisher yDistPub; - private DoublePublisher xDistPub; - private DoublePublisher horizontalDistPub; - - // TODO setup camera IPs? - // https://docs.limelightvision.io/docs/docs-limelight/getting-started/FRC/best-practices - public Limelight(String cameraName, double cameraHeightMeters, double cameraAngle, double cameraOffsetX, - double cameraOffsetY, boolean cameraUpsideDown) { - this.cameraName = cameraName; - this.cameraHeightMeters = cameraHeightMeters; - this.cameraAngle = cameraAngle; - this.cameraOffsetX = cameraOffsetX; - this.cameraOffsetY = cameraOffsetY; - LimelightHelpers.SetFiducialIDFiltersOverride(cameraName, validIDs); - if (cameraUpsideDown) { - angleMult = -1; - } else { - angleMult = 1; - } - - NetworkTableInstance inst = NetworkTableInstance.getDefault(); - NetworkTable lightTable = inst.getTable(cameraName); - - yDistPub = lightTable.getDoubleTopic("Y Distance").publish(); - xDistPub = lightTable.getDoubleTopic("X Distance").publish(); - horizontalDistPub = lightTable.getDoubleTopic("Horizontal Distance").publish(); - } - - // might not be needed - public static boolean isCorrectID(int ID, int... IDs) { - for (int n : IDs) { - if (n == ID) - return true; - } - return false; + /* CONSTANTS */ + + + public static final double TARGET_DEBOUNCE_TIME = 0.2; + + /* INSTANCE VARIABLES */ + private int tagCount; + private int[] validIDs = {}; // TODO: set these + public String cameraName; + private double tx; + private double ty; + private Debouncer targetDebouncer = new Debouncer(TARGET_DEBOUNCE_TIME, DebounceType.kFalling); + + public static final double angleVelocityTolerance = 360 * Math.PI / 180; // in radians per sec + + private double cameraHeightMeters; + public double cameraAngle; + public double cameraOffsetX; // right is positive + public double cameraOffsetY; // forward is positive + private double angleMult; + + private boolean hasTipped; + + private DoublePublisher yDistPub; + private DoublePublisher xDistPub; + private DoublePublisher horizontalDistPub; + + // TODO setup camera IPs? + // https://docs.limelightvision.io/docs/docs-limelight/getting-started/FRC/best-practices + public Limelight( + String cameraName, + double cameraHeightMeters, + double cameraAngle, + double cameraOffsetX, + double cameraOffsetY, + boolean cameraUpsideDown) { + this.cameraName = cameraName; + this.cameraHeightMeters = cameraHeightMeters; + this.cameraAngle = cameraAngle; + this.cameraOffsetX = cameraOffsetX; + this.cameraOffsetY = cameraOffsetY; + LimelightHelpers.SetFiducialIDFiltersOverride(cameraName, validIDs); + if (cameraUpsideDown) { + angleMult = -1; + } else { + angleMult = 1; } - // from last years robot - public double getTimestampSeconds() { - double latency = (LimelightHelpers.getLimelightNTDouble(cameraName, "cl") - + LimelightHelpers.getLimelightNTDouble(cameraName, "tl")) - / 1000.0; + NetworkTableInstance inst = NetworkTableInstance.getDefault(); + NetworkTable lightTable = inst.getTable(cameraName); - return Timer.getFPGATimestamp() - latency; - } + yDistPub = lightTable.getDoubleTopic("Y Distance").publish(); + xDistPub = lightTable.getDoubleTopic("X Distance").publish(); + horizontalDistPub = lightTable.getDoubleTopic("Horizontal Distance").publish(); + } - // from last years robot as well - public boolean hasValidTarget() { - boolean hasMatch = (LimelightHelpers.getLimelightNTDouble(cameraName, "tv") == 1.0); - return targetDebouncer.calculate(hasMatch); - } + // from last years robot + public double getTimestampSeconds() { + double latency = + (LimelightHelpers.getLimelightNTDouble(cameraName, "cl") + + LimelightHelpers.getLimelightNTDouble(cameraName, "tl")) + / 1000.0; - public void disable() { - // https://docs.limelightvision.io/docs/docs-limelight/pipeline-apriltag/apriltag-robot-localization-megatag2#using-limelight-4s-built-in-imu-with-imumode_set--setimumode - // https://docs.limelightvision.io/docs/docs-limelight/software-change-log#limelight-os-20251-final-release---22425-test-release---21825 - LimelightHelpers.SetIMUMode(cameraName, 1); // If not moving reset internal IMU - // LimelightHelpers.setLimelightNTDouble(cameraName, "throttle_set", 200); // - // manage thermals - } + return Timer.getFPGATimestamp() - latency; + } - public void setGyroMode(int mode) { - LimelightHelpers.SetIMUMode(cameraName, mode); - } + // from last years robot as well + public boolean hasValidTarget() { + boolean hasMatch = (LimelightHelpers.getLimelightNTDouble(cameraName, "tv") == 1.0); + return targetDebouncer.calculate(hasMatch); + } - public void enable() { - LimelightHelpers.SetIMUMode(cameraName, 1); // if moving use builtin, maybe change to 4 - // LimelightHelpers.setLimelightNTDouble(cameraName, "throttle_set", 0); //TODO - // check needs to be 1? // manage thermals - } + public void setGyroMode(int mode) { + LimelightHelpers.SetIMUMode(cameraName, mode); + } - public RawFiducial getClosestTag() { - RawFiducial[] tags = LimelightHelpers.getRawFiducials(cameraName); - if (tags.length == 0) { - return null; - } - RawFiducial largest = tags[0]; - for (RawFiducial tag : tags) { - if (tag.distToRobot > largest.distToRobot) { - largest = tag; - } - } - return largest; + public RawFiducial getClosestTag() { + RawFiducial[] tags = LimelightHelpers.getRawFiducials(cameraName); + if (tags.length == 0) { + return null; } - - public Rotation2d getClosestTagAngle() { - int closestId = getClosestTag().id; - return tagRotationsMap.get(closestId); + RawFiducial largest = tags[0]; + for (RawFiducial tag : tags) { + if (tag.distToRobot > largest.distToRobot) { + largest = tag; + } } - - public void poseEstimationMegatag2() { - - double angle = (RobotContainer.drivetrain.getWrappedHeading().getDegrees() + 360) % 360; - LimelightHelpers.SetRobotOrientation(cameraName, angle, 0, 0, 0, 0, 0); - LimelightHelpers.PoseEstimate mt2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(cameraName); - - boolean overrideReject = false; - boolean isTipping = Math.abs(RobotContainer.drivetrain.getPigeon2().getPitch().getValueAsDouble()) > 2 - || Math.abs(RobotContainer.drivetrain.getPigeon2().getRoll().getValueAsDouble()) > 2; - Logger.recordOutput(cameraName + "/isTipping", isTipping); - - if (hasTipped && !isTipping) { - overrideReject = true; - } - - boolean shouldRejectUpdate = false; - int rejectReason = 0; - if (mt2 != null) { - Optional optPastRobotPose = RobotContainer.drivetrain.getPoseAtTime(mt2.timestampSeconds); - if (optPastRobotPose.isPresent()) { - Logger.recordOutput(cameraName + "/PastRobotPose", optPastRobotPose.get()); - } - Pose2d pastRobotPose = RobotContainer.drivetrain.getRobotPose(); - // Pose2d pastRobotPose = optPastRobotPose.orElseGet(() -> RobotContainer.drivetrain.getRobotPose()); - Logger.recordOutput(cameraName + "/timestampSeconds", mt2.timestampSeconds); - RawFiducial[] tags = mt2.rawFiducials; - int[] ids = new int[tags.length]; - for (int i = 0; i < tags.length; i++) { - ids[i] = tags[i].id; - } - Logger.recordOutput(cameraName + "/SeenTags", ids); - Logger.recordOutput(cameraName + "/PoseLatency", mt2.timestampSeconds - Timer.getFPGATimestamp()); - if (mt2.tagCount == 0) { - // rejects current measurement if there are no aprilTags - shouldRejectUpdate = true; - rejectReason = 1; - } - if (Math.abs(RobotContainer.drivetrain.getCurrentSpeeds().omegaRadiansPerSecond) > angleVelocityTolerance) { - shouldRejectUpdate = true; - rejectReason = 2; - } - if ((mt2.pose.getTranslation().getDistance(pastRobotPose.getTranslation()) > 0.9 - && !DriverStation.isDisabled() && !DriverStation.isTeleopEnabled())) { - shouldRejectUpdate = true; - rejectReason = 3; - } - if (Math.abs(PoseUtils.wrapRotation(mt2.pose.getRotation()) - .minus(PoseUtils.wrapRotation(pastRobotPose.getRotation())).getDegrees()) > 3) { - shouldRejectUpdate = true; - rejectReason = 4; - } - if (mt2.avgTagDist > 4) { - shouldRejectUpdate = true; - rejectReason = 5; - } - // if (isTipping) { - // shouldRejectUpdate = true; - // } - // adds vision measurement if conditions are met - if (!shouldRejectUpdate) { - Logger.recordOutput(cameraName + "/mt2Pose", mt2.pose); - Logger.recordOutput(cameraName + "/Calculated stdevs", - Math.pow(0.5, mt2.tagCount) * 2 * mt2.avgTagDist); - // Vector = VecBuilder.fill - RobotContainer.drivetrain.addVisionMeasurement( - mt2.pose, - Utils.fpgaToCurrentTime(mt2.timestampSeconds), - // VecBuilder.fill(0.000716, 0.0003, Double.POSITIVE_INFINITY)); - VecBuilder.fill(Math.pow(0.5, mt2.tagCount) * 2 * mt2.avgTagDist, - Math.pow(0.5, mt2.tagCount) * 2 * mt2.avgTagDist, Double.POSITIVE_INFINITY)); - if (DriverStation.isTeleopEnabled()) { - RobotContainer.leds.playLEDPattern(LEDs.holding(Color.kWhite), 0.2); - } - } else { - Logger.recordOutput(cameraName + "/mt2PoseRejected", mt2.pose); - Logger.recordOutput(cameraName + "/rejectReason", rejectReason); - } - } + return largest; + } + + public void poseEstimationMegatag2() { + + double angle = (RobotContainer.drivetrain.getWrappedHeading().getDegrees() + 360) % 360; + LimelightHelpers.SetRobotOrientation(cameraName, angle, 0, 0, 0, 0, 0); + LimelightHelpers.PoseEstimate mt2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(cameraName); + + boolean shouldRejectUpdate = false; + + int rejectReason = 0; + + if (mt2 != null) { + Optional optPastRobotPose = + RobotContainer.drivetrain.getPoseAtTime(mt2.timestampSeconds); + if (optPastRobotPose.isPresent()) { + Logger.recordOutput(cameraName + "/PastRobotPose", optPastRobotPose.get()); + } + Pose2d pastRobotPose = RobotContainer.drivetrain.getRobotPose(); + // Pose2d pastRobotPose = optPastRobotPose.orElseGet(() -> + // RobotContainer.drivetrain.getRobotPose()); + Logger.recordOutput(cameraName + "/timestampSeconds", mt2.timestampSeconds); + RawFiducial[] tags = mt2.rawFiducials; + int[] ids = new int[tags.length]; + for (int i = 0; i < tags.length; i++) { + ids[i] = tags[i].id; + } + Logger.recordOutput(cameraName + "/SeenTags", ids); + Logger.recordOutput( + cameraName + "/PoseLatency", mt2.timestampSeconds - Timer.getFPGATimestamp()); + if (mt2.tagCount == 0) { + // rejects current measurement if there are no aprilTags + shouldRejectUpdate = true; + rejectReason = 1; + } + if (Math.abs(RobotContainer.drivetrain.getCurrentSpeeds().omegaRadiansPerSecond) + > angleVelocityTolerance) { + shouldRejectUpdate = true; + rejectReason = 2; + } + if ((mt2.pose.getTranslation().getDistance(pastRobotPose.getTranslation()) > 0.9 + && !DriverStation.isDisabled() + && !DriverStation.isTeleopEnabled())) { + shouldRejectUpdate = true; + rejectReason = 3; + } + if (Math.abs( + PoseUtils.wrapRotation(mt2.pose.getRotation()) + .minus(PoseUtils.wrapRotation(pastRobotPose.getRotation())) + .getDegrees()) + > 3) { + shouldRejectUpdate = true; + rejectReason = 4; + } + if (mt2.avgTagDist > 4) { + shouldRejectUpdate = true; + rejectReason = 5; + } + + // adds vision measurement if conditions are met + if (!shouldRejectUpdate) { + Logger.recordOutput(cameraName + "/mt2Pose", mt2.pose); + Logger.recordOutput( + cameraName + "/Calculated stdevs", Math.pow(0.5, mt2.tagCount) * 2 * mt2.avgTagDist); + // Vector = VecBuilder.fill + RobotContainer.drivetrain.addVisionMeasurement( + mt2.pose, + Utils.fpgaToCurrentTime(mt2.timestampSeconds), + // VecBuilder.fill(0.000716, 0.0003, Double.POSITIVE_INFINITY)); + VecBuilder.fill( + Math.pow(0.5, mt2.tagCount) * 2 * mt2.avgTagDist, + Math.pow(0.5, mt2.tagCount) * 2 * mt2.avgTagDist, + Double.POSITIVE_INFINITY)); + } else { + Logger.recordOutput(cameraName + "/mt2PoseRejected", mt2.pose); + Logger.recordOutput(cameraName + "/rejectReason", rejectReason); + } } + } - // TODO: Do we need these / check if the trig is right + // TODO: Do we need these / check if the trig is right - public double getDistanceToTag(double tagHeightMeters) { - if (hasValidTarget()) { - double distance = getStraightDistanceToTag(tagHeightMeters) - cameraOffsetY; - return distance / Math.cos((Math.PI / 180.0) * getTX()); - } - return 0; + public double getDistanceToTag(double tagHeightMeters) { + if (hasValidTarget()) { + double distance = getStraightDistanceToTag(tagHeightMeters) - cameraOffsetY; + return distance / Math.cos((Math.PI / 180.0) * getTX()); } - - public double getStraightDistanceToTag(double tagHeightMeters) { - if (hasValidTarget()) { - double distance = (tagHeightMeters - cameraHeightMeters) - / Math.tan( - (Math.PI / 180.0) - * (cameraAngle + getTY())); - return distance + cameraOffsetY; - } - return 0; + return 0; + } + + public double getStraightDistanceToTag(double tagHeightMeters) { + if (hasValidTarget()) { + double distance = + (tagHeightMeters - cameraHeightMeters) + / Math.tan((Math.PI / 180.0) * (cameraAngle + getTY())); + return distance + cameraOffsetY; } + return 0; + } - public double getHorizontalDistanceToTag(double tagHeightMeters) { - if (hasValidTarget()) { - double distance = getStraightDistanceToTag(tagHeightMeters) - cameraOffsetY; + public double getHorizontalDistanceToTag(double tagHeightMeters) { + if (hasValidTarget()) { + double distance = getStraightDistanceToTag(tagHeightMeters) - cameraOffsetY; - distance = distance * Math.tan(getTX() * (Math.PI / 180.0)); - return distance + cameraOffsetX; - } - return 0; + distance = distance * Math.tan(getTX() * (Math.PI / 180.0)); + return distance + cameraOffsetX; } + return 0; + } - public double getDistanceToCoralStation() { - return getDistanceToTag(coralStationTagHeightMeters); - } - public double getStraightDistanceToCoralStation() { - return getStraightDistanceToTag(coralStationTagHeightMeters); + @AutoLogOutput + public double getTX() { + return tx * angleMult; + } - } + @AutoLogOutput + public double getTY() { + return ty * -angleMult; + } - public double getHorizontalDistanceToCoralStation() { - return getHorizontalDistanceToTag(coralStationTagHeightMeters); - } + public DoubleSupplier tySupplier() { + return () -> getTY(); + } - // ISN'T OFFSET FOR THE CENTER OF THE ROBOT!!!!!!! - public double getDistanceToReef() { - return getDistanceToTag(reefTagHeightMeters); - } + public DoubleSupplier txSupplier() { + return () -> getTX(); + } - // TODO: Do we need these / check if the trig is right - public double getStraightDistanceToReef() { - return getStraightDistanceToTag(reefTagHeightMeters); - } + // TODO: Do we need these / check if the trig is right + // public double getStraightDistanceToTag() { + // if (hasValidTarget()) + // return goalHeightReef / (Math.tan(Math.toRadians(getTY() + + // limelightOffsetAngleVertical))); + // return 0; + // } - public double getHorizontalDistanceToReef() { - return getHorizontalDistanceToTag(reefTagHeightMeters); - } + // TODO: Do we need these / check if the trig is right - @AutoLogOutput - public double getTX() { - return tx * angleMult; - } - @AutoLogOutput - public double getTY() { - return ty * -angleMult; - } + public int getTagID() { + return (int) LimelightHelpers.getFiducialID(cameraName); + } - public DoubleSupplier tySupplier() { - return () -> getTY(); - } - - public DoubleSupplier txSupplier() { - return () -> getTX(); - } + public void periodic() { - // TODO: Do we need these / check if the trig is right - // public double getStraightDistanceToTag() { - // if (hasValidTarget()) - // return goalHeightReef / (Math.tan(Math.toRadians(getTY() + - // limelightOffsetAngleVertical))); - // return 0; - // } - - // TODO: Do we need these / check if the trig is right - public double getStrafeDistanceToReef() { - if (isCorrectID(getTagID(), reefIDs)) { - return (Math.tan(Math.toRadians(getTX()))) * getStraightDistanceToReef(); - } - return 0; + if (Math.abs(RobotContainer.drivetrain.getPigeon2().getPitch().getValueAsDouble()) > 0.3 + || Math.abs(RobotContainer.drivetrain.getPigeon2().getRoll().getValueAsDouble()) > 0.3) { + hasTipped = true; } - - public int getTagID() { - return (int) LimelightHelpers.getFiducialID(cameraName); + // tagID = (int) Limetable.getEntry("tid").getDouble(-1); + // TODO if you get a pose estimate in the frame before this is applied it may + // not work + tx = LimelightHelpers.getTX(cameraName); + ty = LimelightHelpers.getTY(cameraName); + RawFiducial[] allTags = LimelightHelpers.getRawFiducials(cameraName); + int numValidTags = 0; + for (LimelightHelpers.RawFiducial t : allTags) { + if (t.distToCamera < 4.0) { + numValidTags++; + } } - public void periodic() { - - if (Math.abs(RobotContainer.drivetrain.getPigeon2().getPitch().getValueAsDouble()) > 0.3 - || Math.abs(RobotContainer.drivetrain.getPigeon2().getRoll().getValueAsDouble()) > 0.3) { - hasTipped = true; - } - // tagID = (int) Limetable.getEntry("tid").getDouble(-1); - // TODO if you get a pose estimate in the frame before this is applied it may - // not work - tx = LimelightHelpers.getTX(cameraName); - ty = LimelightHelpers.getTY(cameraName); - RawFiducial[] allTags = LimelightHelpers.getRawFiducials(cameraName); - int numValidTags = 0; - for (LimelightHelpers.RawFiducial t : allTags) { - if (t.distToCamera < 4.0) { - numValidTags++; - } - } - - int[] validTags = new int[numValidTags]; - int counter = 0; - for (RawFiducial t : allTags) { - if (t.distToCamera < 4.0) { - validTags[counter] = t.id; - counter++; - } - } - // LimelightHelpers.SetFiducialIDFiltersOverride(cameraName, validTags); - poseEstimationMegatag2(); - xDistPub.set(getHorizontalDistanceToReef()); - yDistPub.set(getStraightDistanceToReef()); - horizontalDistPub.set(getDistanceToReef()); - - double[] poseArr = LimelightHelpers.getBotPose_TargetSpace(cameraName); - Pose2d botPose = new Pose2d(); - if (poseArr.length >= 6) { - botPose = new Pose2d(poseArr[0], poseArr[2], Rotation2d.fromDegrees(poseArr[4])); - } - Logger.recordOutput(cameraName + "/IMUYaw", - LimelightHelpers.getIMUData(cameraName).robotYaw * (Math.PI / 180.0)); // TODO should be yaw? - Logger.recordOutput(cameraName + "/BotPoseTargetSpace", botPose); - Logger.recordOutput(cameraName + "/BotPose3dTargetSpace", - LimelightHelpers.getBotPose3d_TargetSpace(cameraName)); - - var entry = LimelightHelpers.getLimelightNTTableEntry(cameraName, "tcornxy"); - if (entry != null) { - var tcornxy = entry.getDoubleArray(new double[0]); - if (tcornxy != null && tcornxy.length > 0) { - Logger.recordOutput(cameraName + "/tcornxy", tcornxy); - } - } + int[] validTags = new int[numValidTags]; + int counter = 0; + for (RawFiducial t : allTags) { + if (t.distToCamera < 4.0) { + validTags[counter] = t.id; + counter++; + } } - public Command flashLEDs() { - return Commands.sequence( - Commands.runOnce(() -> LimelightHelpers.setLEDMode_ForceBlink(cameraName)), - Commands.waitSeconds(0.6), - Commands.runOnce(() -> LimelightHelpers.setLEDMode_ForceOff(cameraName)) - ); + double[] poseArr = LimelightHelpers.getBotPose_TargetSpace(cameraName); + Pose2d botPose = new Pose2d(); + if (poseArr.length >= 6) { + botPose = new Pose2d(poseArr[0], poseArr[2], Rotation2d.fromDegrees(poseArr[4])); } - - - - public Command ifHasTarget(Command cmd) { - return cmd.onlyWhile(this::hasValidTarget); + Logger.recordOutput( + cameraName + "/IMUYaw", + LimelightHelpers.getIMUData(cameraName).robotYaw + * (Math.PI / 180.0)); // TODO should be yaw? + Logger.recordOutput(cameraName + "/BotPoseTargetSpace", botPose); + Logger.recordOutput( + cameraName + "/BotPose3dTargetSpace", + LimelightHelpers.getBotPose3d_TargetSpace(cameraName)); + + var entry = LimelightHelpers.getLimelightNTTableEntry(cameraName, "tcornxy"); + if (entry != null) { + var tcornxy = entry.getDoubleArray(new double[0]); + if (tcornxy != null && tcornxy.length > 0) { + Logger.recordOutput(cameraName + "/tcornxy", tcornxy); + } } + } + + public Command flashLEDs() { + return Commands.sequence( + Commands.runOnce(() -> LimelightHelpers.setLEDMode_ForceBlink(cameraName)), + Commands.waitSeconds(0.6), + Commands.runOnce(() -> LimelightHelpers.setLEDMode_ForceOff(cameraName))); + } + + public Command ifHasTarget(Command cmd) { + return cmd.onlyWhile(this::hasValidTarget); + } } - diff --git a/src/main/java/frc/robot/subsystems/vision/LimelightHelpers.java b/src/main/java/frc/robot/subsystems/vision/LimelightHelpers.java new file mode 100644 index 00000000..82ad983e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/LimelightHelpers.java @@ -0,0 +1,1647 @@ +//LimelightHelpers v1.11 (REQUIRES LLOS 2025.0 OR LATER) + +package frc.robot.subsystems.vision; + +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.vision.LimelightHelpers.LimelightResults; +import frc.robot.subsystems.vision.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; + } + + } + + /** + * 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]; + } + } + } + + + private static ObjectMapper mapper; + + /** + * Print JSON Parse time to the console in milliseconds + */ + static boolean profileJSON = false; + + static final String sanitizeName(String name) { + if ("".equals(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); + + } + + /** + * 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); + } + + ///// + ///// + + 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(); + } + } + + /** + * Configures the IMU mode for MegaTag2 Localization + * + * @param limelightName Name/identifier of the Limelight + * @param mode IMU mode. + */ + public static void SetIMUMode(String limelightName, int mode) { + setLimelightNTDouble(limelightName, "imumode_set", mode); + } + + /** + * 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 && !"".equals(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; + } +} \ No newline at end of file From 06b811db0dbc699a9a999d72c6605c26c15832e6 Mon Sep 17 00:00:00 2001 From: sub0dev <125705137+Mr-Pyro@users.noreply.github.com> Date: Tue, 13 Jan 2026 16:54:50 -0500 Subject: [PATCH 4/9] Added --- .../robot/subsystems/vision/Limelight.java | 21 ++++++++++++++++++- 1 file changed, 20 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/vision/Limelight.java b/src/main/java/frc/robot/subsystems/vision/Limelight.java index 95172e2f..6894c631 100644 --- a/src/main/java/frc/robot/subsystems/vision/Limelight.java +++ b/src/main/java/frc/robot/subsystems/vision/Limelight.java @@ -26,7 +26,26 @@ public class Limelight extends SubsystemBase { /* CONSTANTS */ + public static final double hubTagHeightMeters = 1.12395; + public static final double trenchTagHeightMeters = 0.889; + public static final double towerTagHeightMeters = 0.55245; + public static final double outpostTagHeightMeters = 0.55245; + public static final int[] hubIDsRed = { 2, 3, 4, 5, 8, 9, 10, 11 }; + public static final int[] hubIDsBlue = { 18, 19, 20, 21, 24, 25, 26, 27 }; + public static final int[] reefIDs = { 2, 3, 4, 5, 8, 9, 10, 11, 18, 19, 20, 21, 24, 25, 26, 27 }; + + public static final int[] towerIDsRed = { 15, 16 }; + public static final int[] towerIDsBlue = { 31, 32 }; + public static final int[] towerStationIDs = { 15, 16, 31, 32 }; + + public static final int[] trenchIDsRed = { 1, 6, 7, 12}; + public static final int[] trenchIDsBlue = { 17, 22, 23, 28 }; + public static final int[] trenchStationIDs = { 1, 6, 7, 12, 17, 22, 23, 28}; + + public static final int[] outpostIDsRed = { 15, 16 }; + public static final int[] outpostIDsBlue = { 31, 32 }; + public static final int[] outpostStationIDs = { 15, 16, 31, 32 }; public static final double TARGET_DEBOUNCE_TIME = 0.2; @@ -256,7 +275,7 @@ public DoubleSupplier txSupplier() { public int getTagID() { return (int) LimelightHelpers.getFiducialID(cameraName); } - + public void periodic() { if (Math.abs(RobotContainer.drivetrain.getPigeon2().getPitch().getValueAsDouble()) > 0.3 From 2e3c5b3bb2e6565c2f2ce9754290f5c6c3190222 Mon Sep 17 00:00:00 2001 From: sub0dev <125705137+Mr-Pyro@users.noreply.github.com> Date: Wed, 14 Jan 2026 15:18:16 -0500 Subject: [PATCH 5/9] Added getDistanceToHub and getAngleToHub methods ask cad for location of hubs --- .../robot/subsystems/vision/Limelight.java | 23 +++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/vision/Limelight.java b/src/main/java/frc/robot/subsystems/vision/Limelight.java index 6894c631..d4642dc4 100644 --- a/src/main/java/frc/robot/subsystems/vision/Limelight.java +++ b/src/main/java/frc/robot/subsystems/vision/Limelight.java @@ -22,6 +22,7 @@ import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; +import edu.wpi.first.math.geometry.Translation2d; public class Limelight extends SubsystemBase { @@ -276,6 +277,28 @@ public int getTagID() { return (int) LimelightHelpers.getFiducialID(cameraName); } + public double getDistanceToHub(){ + Pose2d robotPose = RobotContainer.drivetrain.getRobotPose(); + Translation2d hubLocation = new Translation2d(8.27, 4.105); + //TODO:Set alliance translations to their true locations + if (DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == DriverStation.Alliance.Red) { + hubLocation = new Translation2d(8.27, 4.105); + } + Pose2d hubPose = new Pose2d(hubLocation, Rotation2d.fromDegrees(0)); + return robotPose.getTranslation().getDistance(hubPose.getTranslation()); + } + + public double getAngleToHub(){ + Pose2d robotPose = RobotContainer.drivetrain.getRobotPose(); + Translation2d hubLocation = new Translation2d(8.27, 4.105); + //TODO:Set alliance translations to their true locations + if (DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == DriverStation.Alliance.Red) { + hubLocation = new Translation2d(8.27, 4.105); + } + Translation2d robotToHub = hubLocation.minus(robotPose.getTranslation()); + return robotToHub.getAngle().getDegrees(); + } + public void periodic() { if (Math.abs(RobotContainer.drivetrain.getPigeon2().getPitch().getValueAsDouble()) > 0.3 From 36d3dda21dc34536a76858d2a8f513f05691dea4 Mon Sep 17 00:00:00 2001 From: sub0dev <125705137+Mr-Pyro@users.noreply.github.com> Date: Thu, 15 Jan 2026 16:08:03 -0500 Subject: [PATCH 6/9] Added Real Constants, FlipPose util --- simgui-ds.json | 92 + src/main/java/frc/robot/BuildConstants.java | 12 +- .../drivetrain/CommandSwerveDrivetrain.java | 11 +- .../robot/subsystems/vision/Limelight.java | 65 +- .../subsystems/vision/LimelightHelpers.java | 3081 +++++++++-------- 5 files changed, 1696 insertions(+), 1565 deletions(-) create mode 100644 simgui-ds.json diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 00000000..73cc713c --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,92 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ] +} diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index c15a016e..34e19256 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,13 +5,13 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Rebuilt"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 4; - public static final String GIT_SHA = "5257b345fb5c3a43fa51ec15bb28b16a2a6abab5"; - public static final String GIT_DATE = "2026-01-12 16:23:13 EST"; + public static final int GIT_REVISION = 7; + public static final String GIT_SHA = "2e3c5b3bb2e6565c2f2ce9754290f5c6c3190222"; + public static final String GIT_DATE = "2026-01-14 15:18:16 EST"; public static final String GIT_BRANCH = "vision"; - public static final String BUILD_DATE = "2026-01-12 16:25:45 EST"; - public static final long BUILD_UNIX_TIME = 1768253145169L; - public static final int DIRTY = 1; + public static final String BUILD_DATE = "2026-01-14 16:26:24 EST"; + public static final long BUILD_UNIX_TIME = 1768425984934L; + public static final int DIRTY = 0; private BuildConstants() {} } diff --git a/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java index e431ab7b..63cc1d0a 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java @@ -40,7 +40,6 @@ import frc.robot.util.PoseUtils; import frc.robot.util.TunerConstants; import frc.robot.util.TunerConstants.TunerSwerveDrivetrain; - import java.util.Optional; import java.util.function.Supplier; import org.littletonrobotics.junction.AutoLogOutput; @@ -49,7 +48,6 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Subsystem { public TimeInterpolatableBuffer poseBuffer = TimeInterpolatableBuffer.createBuffer(3); - private static final double kSimLoopPeriod = 0.002; // 2 ms private Notifier m_simNotifier = null; @@ -393,12 +391,15 @@ public Command alignToAngleFieldRelativeCommand(Rotation2d angle, boolean lockDr Commands.run(() -> alignToAngleFieldRelative(lockDrive), this) .until(() -> isRobotAtAngleSetPoint)); } + public ChassisSpeeds getCurrentSpeeds() { - return this.getState().Speeds; + return this.getState().Speeds; } + public Optional getPoseAtTime(double time) { - return poseBuffer.getSample(time); - } + return poseBuffer.getSample(time); + } + @Override public void periodic() { diff --git a/src/main/java/frc/robot/subsystems/vision/Limelight.java b/src/main/java/frc/robot/subsystems/vision/Limelight.java index d4642dc4..f1c46365 100644 --- a/src/main/java/frc/robot/subsystems/vision/Limelight.java +++ b/src/main/java/frc/robot/subsystems/vision/Limelight.java @@ -6,6 +6,7 @@ import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.networktables.DoublePublisher; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; @@ -15,38 +16,36 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.RobotContainer; -import frc.robot.util.PoseUtils; import frc.robot.subsystems.vision.LimelightHelpers.RawFiducial; -import java.util.HashMap; +import frc.robot.util.PoseUtils; import java.util.Optional; import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; -import edu.wpi.first.math.geometry.Translation2d; public class Limelight extends SubsystemBase { /* CONSTANTS */ - public static final double hubTagHeightMeters = 1.12395; + public static final double hubTagHeightMeters = 1.12395; public static final double trenchTagHeightMeters = 0.889; - public static final double towerTagHeightMeters = 0.55245; + public static final double towerTagHeightMeters = 0.55245; public static final double outpostTagHeightMeters = 0.55245; - public static final int[] hubIDsRed = { 2, 3, 4, 5, 8, 9, 10, 11 }; - public static final int[] hubIDsBlue = { 18, 19, 20, 21, 24, 25, 26, 27 }; - public static final int[] reefIDs = { 2, 3, 4, 5, 8, 9, 10, 11, 18, 19, 20, 21, 24, 25, 26, 27 }; + public static final int[] hubIDsRed = {2, 3, 4, 5, 8, 9, 10, 11}; + public static final int[] hubIDsBlue = {18, 19, 20, 21, 24, 25, 26, 27}; + public static final int[] reefIDs = {2, 3, 4, 5, 8, 9, 10, 11, 18, 19, 20, 21, 24, 25, 26, 27}; - public static final int[] towerIDsRed = { 15, 16 }; - public static final int[] towerIDsBlue = { 31, 32 }; - public static final int[] towerStationIDs = { 15, 16, 31, 32 }; + public static final int[] towerIDsRed = {15, 16}; + public static final int[] towerIDsBlue = {31, 32}; + public static final int[] towerStationIDs = {15, 16, 31, 32}; - public static final int[] trenchIDsRed = { 1, 6, 7, 12}; - public static final int[] trenchIDsBlue = { 17, 22, 23, 28 }; - public static final int[] trenchStationIDs = { 1, 6, 7, 12, 17, 22, 23, 28}; + public static final int[] trenchIDsRed = {1, 6, 7, 12}; + public static final int[] trenchIDsBlue = {17, 22, 23, 28}; + public static final int[] trenchStationIDs = {1, 6, 7, 12, 17, 22, 23, 28}; - public static final int[] outpostIDsRed = { 15, 16 }; - public static final int[] outpostIDsBlue = { 31, 32 }; - public static final int[] outpostStationIDs = { 15, 16, 31, 32 }; + public static final int[] outpostIDsRed = {15, 16}; + public static final int[] outpostIDsBlue = {31, 32}; + public static final int[] outpostStationIDs = {15, 16, 31, 32}; public static final double TARGET_DEBOUNCE_TIME = 0.2; @@ -139,7 +138,8 @@ public void poseEstimationMegatag2() { double angle = (RobotContainer.drivetrain.getWrappedHeading().getDegrees() + 360) % 360; LimelightHelpers.SetRobotOrientation(cameraName, angle, 0, 0, 0, 0, 0); - LimelightHelpers.PoseEstimate mt2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(cameraName); + LimelightHelpers.PoseEstimate mt2 = + LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(cameraName); boolean shouldRejectUpdate = false; @@ -191,7 +191,7 @@ public void poseEstimationMegatag2() { shouldRejectUpdate = true; rejectReason = 5; } - + // adds vision measurement if conditions are met if (!shouldRejectUpdate) { Logger.recordOutput(cameraName + "/mt2Pose", mt2.pose); @@ -243,7 +243,6 @@ public double getHorizontalDistanceToTag(double tagHeightMeters) { return 0; } - @AutoLogOutput public double getTX() { return tx * angleMult; @@ -272,31 +271,25 @@ public DoubleSupplier txSupplier() { // TODO: Do we need these / check if the trig is right - public int getTagID() { return (int) LimelightHelpers.getFiducialID(cameraName); } - - public double getDistanceToHub(){ + + public double getDistanceToHub() { Pose2d robotPose = RobotContainer.drivetrain.getRobotPose(); - Translation2d hubLocation = new Translation2d(8.27, 4.105); - //TODO:Set alliance translations to their true locations - if (DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == DriverStation.Alliance.Red) { - hubLocation = new Translation2d(8.27, 4.105); - } + Translation2d hubLocation = new Translation2d(4.6245018, 4.105); Pose2d hubPose = new Pose2d(hubLocation, Rotation2d.fromDegrees(0)); + hubPose = PoseUtils.flipPoseAlliance(hubPose); return robotPose.getTranslation().getDistance(hubPose.getTranslation()); } - public double getAngleToHub(){ + public double getAngleToHub() { Pose2d robotPose = RobotContainer.drivetrain.getRobotPose(); - Translation2d hubLocation = new Translation2d(8.27, 4.105); - //TODO:Set alliance translations to their true locations - if (DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == DriverStation.Alliance.Red) { - hubLocation = new Translation2d(8.27, 4.105); - } - Translation2d robotToHub = hubLocation.minus(robotPose.getTranslation()); - return robotToHub.getAngle().getDegrees(); + Translation2d hubLocation = new Translation2d(4.6245018, 4.105); + Pose2d hubPose = new Pose2d(hubLocation, Rotation2d.fromDegrees(0)); + hubPose = PoseUtils.flipPoseAlliance(hubPose); + Translation2d robotToHub = hubPose.getTranslation().minus(robotPose.getTranslation()); + return robotToHub.getAngle().getRotations(); } public void periodic() { diff --git a/src/main/java/frc/robot/subsystems/vision/LimelightHelpers.java b/src/main/java/frc/robot/subsystems/vision/LimelightHelpers.java index 82ad983e..cb6758c0 100644 --- a/src/main/java/frc/robot/subsystems/vision/LimelightHelpers.java +++ b/src/main/java/frc/robot/subsystems/vision/LimelightHelpers.java @@ -1,7 +1,20 @@ -//LimelightHelpers v1.11 (REQUIRES LLOS 2025.0 OR LATER) +// LimelightHelpers v1.11 (REQUIRES LLOS 2025.0 OR LATER) package frc.robot.subsystems.vision; +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 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.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.DoubleArrayEntry; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; @@ -9,1639 +22,1671 @@ import edu.wpi.first.networktables.TimestampedDoubleArray; import frc.robot.subsystems.vision.LimelightHelpers.LimelightResults; import frc.robot.subsystems.vision.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. + * 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; + private static final Map doubleArrayEntries = new ConcurrentHashMap<>(); - @JsonProperty("ta") - public double ta; + /** Represents a Color/Retroreflective Target Result extracted from JSON Output */ + public static class LimelightTarget_Retro { - @JsonProperty("pts") - public double[][] corners; + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; - public LimelightTarget_Barcode() { - } + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; - public String getFamily() { - return family; - } - } - - /** - * Represents a Neural Classifier Pipeline Result extracted from JSON Output - */ - public static class LimelightTarget_Classifier { + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; - @JsonProperty("class") - public String className; + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; - @JsonProperty("classID") - public double classID; + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; - @JsonProperty("conf") - public double confidence; + public Pose3d getCameraPose_TargetSpace() { + return toPose3D(cameraPose_TargetSpace); + } - @JsonProperty("zone") - public double zone; + public Pose3d getRobotPose_FieldSpace() { + return toPose3D(robotPose_FieldSpace); + } - @JsonProperty("tx") - public double tx; + public Pose3d getRobotPose_TargetSpace() { + return toPose3D(robotPose_TargetSpace); + } - @JsonProperty("txp") - public double tx_pixels; + public Pose3d getTargetPose_CameraSpace() { + return toPose3D(targetPose_CameraSpace); + } - @JsonProperty("ty") - public double ty; + public Pose3d getTargetPose_RobotSpace() { + return toPose3D(targetPose_RobotSpace); + } - @JsonProperty("typ") - public double ty_pixels; + public Pose2d getCameraPose_TargetSpace2D() { + return toPose2D(cameraPose_TargetSpace); + } - public LimelightTarget_Classifier() { - } + public Pose2d getRobotPose_FieldSpace2D() { + return toPose2D(robotPose_FieldSpace); } - /** - * Represents a Neural Detector Pipeline Result extracted from JSON Output - */ - public static class LimelightTarget_Detector { + public Pose2d getRobotPose_TargetSpace2D() { + return toPose2D(robotPose_TargetSpace); + } - @JsonProperty("class") - public String className; + public Pose2d getTargetPose_CameraSpace2D() { + return toPose2D(targetPose_CameraSpace); + } - @JsonProperty("classID") - public double classID; + public Pose2d getTargetPose_RobotSpace2D() { + return toPose2D(targetPose_RobotSpace); + } - @JsonProperty("conf") - public double confidence; + @JsonProperty("ta") + public double ta; - @JsonProperty("ta") - public double ta; + @JsonProperty("tx") + public double tx; - @JsonProperty("tx") - public double tx; + @JsonProperty("ty") + public double ty; - @JsonProperty("ty") - public double ty; + @JsonProperty("txp") + public double tx_pixels; - @JsonProperty("txp") - public double tx_pixels; + @JsonProperty("typ") + public double ty_pixels; - @JsonProperty("typ") - public double ty_pixels; + @JsonProperty("tx_nocross") + public double tx_nocrosshair; - @JsonProperty("tx_nocross") - public double tx_nocrosshair; + @JsonProperty("ty_nocross") + public double ty_nocrosshair; - @JsonProperty("ty_nocross") - public double ty_nocrosshair; + @JsonProperty("ts") + public double ts; - public LimelightTarget_Detector() { - } + 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]; } + } - /** - * Limelight Results object, parsed from a Limelight's JSON results output. - */ - public static class LimelightResults { - - public String error; - - @JsonProperty("pID") - public double pipelineID; + /** Represents an AprilTag/Fiducial Target Result extracted from JSON Output */ + public static class LimelightTarget_Fiducial { - @JsonProperty("tl") - public double latency_pipeline; + @JsonProperty("fID") + public double fiducialID; - @JsonProperty("cl") - public double latency_capture; + @JsonProperty("fam") + public String fiducialFamily; - public double latency_jsonParse; + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; - @JsonProperty("ts") - public double timestamp_LIMELIGHT_publish; + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; - @JsonProperty("ts_rio") - public double timestamp_RIOFPGA_capture; + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; - @JsonProperty("v") - @JsonFormat(shape = Shape.NUMBER) - public boolean valid; + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; - @JsonProperty("botpose") - public double[] botpose; + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; - @JsonProperty("botpose_wpired") - public double[] botpose_wpired; + public Pose3d getCameraPose_TargetSpace() { + return toPose3D(cameraPose_TargetSpace); + } - @JsonProperty("botpose_wpiblue") - public double[] botpose_wpiblue; + public Pose3d getRobotPose_FieldSpace() { + return toPose3D(robotPose_FieldSpace); + } - @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; - } - - } - - /** - * 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]; - } - } - } - - - private static ObjectMapper mapper; - - /** - * Print JSON Parse time to the console in milliseconds - */ - static boolean profileJSON = false; - - static final String sanitizeName(String name) { - if ("".equals(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 Pose3d getRobotPose_TargetSpace() { + return toPose3D(robotPose_TargetSpace); } - - public static double getLimelightNTDouble(String tableName, String entryName) { - return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0); + + public Pose3d getTargetPose_CameraSpace() { + return toPose3D(targetPose_CameraSpace); } - public static void setLimelightNTDouble(String tableName, String entryName, double val) { - getLimelightNTTableEntry(tableName, entryName).setDouble(val); + public Pose3d getTargetPose_RobotSpace() { + return toPose3D(targetPose_RobotSpace); } - public static void setLimelightNTDoubleArray(String tableName, String entryName, double[] val) { - getLimelightNTTableEntry(tableName, entryName).setDoubleArray(val); + public Pose2d getCameraPose_TargetSpace2D() { + return toPose2D(cameraPose_TargetSpace); } - public static double[] getLimelightNTDoubleArray(String tableName, String entryName) { - return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]); + public Pose2d getRobotPose_FieldSpace2D() { + return toPose2D(robotPose_FieldSpace); } + public Pose2d getRobotPose_TargetSpace2D() { + return toPose2D(robotPose_TargetSpace); + } - public static String getLimelightNTString(String tableName, String entryName) { - return getLimelightNTTableEntry(tableName, entryName).getString(""); + public Pose2d getTargetPose_CameraSpace2D() { + return toPose2D(targetPose_CameraSpace); } - public static String[] getLimelightNTStringArray(String tableName, String entryName) { - return getLimelightNTTableEntry(tableName, entryName).getStringArray(new String[0]); + public Pose2d getTargetPose_RobotSpace2D() { + return toPose2D(targetPose_RobotSpace); } + @JsonProperty("ta") + public double ta; - 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; - } - ///// - ///// + @JsonProperty("tx") + public double tx; - /** - * 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"); - } + @JsonProperty("ty") + public double ty; - /** - * 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"); - } + @JsonProperty("txp") + public double tx_pixels; - /** - * 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"); - } + @JsonProperty("typ") + public double ty_pixels; - /** - * 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"); - } + @JsonProperty("tx_nocross") + public double tx_nocrosshair; - /** - * 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"); - } + @JsonProperty("ty_nocross") + public double ty_nocrosshair; - /** - * 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"); - } + @JsonProperty("ts") + public double ts; - /** - * 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 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 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; - } + /** Represents a Barcode Target Result extracted from JSON Output */ + public static class LimelightTarget_Barcode { - /** - * 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; - } + /** Barcode family type (e.g. "QR", "DataMatrix", etc.) */ + @JsonProperty("fam") + public String family; - /** - * 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 decoded data content of the barcode */ + @JsonProperty("data") + public String data; - /** - * 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"); - } + @JsonProperty("txp") + public double tx_pixels; - /** - * 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"); - } + @JsonProperty("typ") + public double ty_pixels; - /** - * 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"); - } + @JsonProperty("tx") + public double tx; - /** - * 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"); - } + @JsonProperty("ty") + public double ty; - /** - * 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"); - } + @JsonProperty("tx_nocross") + public double tx_nocrosshair; - /** - * 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"); - } + @JsonProperty("ty_nocross") + public double ty_nocrosshair; - /** - * Switch to getBotPose - * - * @param limelightName - * @return - */ - @Deprecated - public static double[] getBotpose(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose"); - } + @JsonProperty("ta") + public double ta; - /** - * Switch to getBotPose_wpiRed - * - * @param limelightName - * @return - */ - @Deprecated - public static double[] getBotpose_wpiRed(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); - } + @JsonProperty("pts") + public double[][] corners; - /** - * Switch to getBotPose_wpiBlue - * - * @param limelightName - * @return - */ - @Deprecated - public static double[] getBotpose_wpiBlue(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); - } + public LimelightTarget_Barcode() {} - public static double[] getBotPose(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose"); + public String getFamily() { + return family; } + } - public static double[] getBotPose_wpiRed(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); - } + /** Represents a Neural Classifier Pipeline Result extracted from JSON Output */ + public static class LimelightTarget_Classifier { - public static double[] getBotPose_wpiBlue(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); - } + @JsonProperty("class") + public String className; - public static double[] getBotPose_TargetSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); - } + @JsonProperty("classID") + public double classID; - public static double[] getCameraPose_TargetSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); - } + @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 static double[] getTargetPose_CameraSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + 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 static double[] getTargetPose_RobotSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + public Pose3d getBotPose3d_wpiRed() { + return toPose3D(botpose_wpired); } - public static double[] getTargetColor(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "tc"); + public Pose3d getBotPose3d_wpiBlue() { + return toPose3D(botpose_wpiblue); } - public static double getFiducialID(String limelightName) { - return getLimelightNTDouble(limelightName, "tid"); + public Pose2d getBotPose2d() { + return toPose2D(botpose); } - public static String getNeuralClassID(String limelightName) { - return getLimelightNTString(limelightName, "tclass"); + public Pose2d getBotPose2d_wpiRed() { + return toPose2D(botpose_wpired); } - public static String[] getRawBarcodeData(String limelightName) { - return getLimelightNTStringArray(limelightName, "rawbarcodes"); + public Pose2d getBotPose2d_wpiBlue() { + return toPose2D(botpose_wpiblue); } - ///// - ///// + @JsonProperty("Retro") + public LimelightTarget_Retro[] targets_Retro; - public static Pose3d getBotPose3d(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose"); - return toPose3D(poseArray); + @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; + } + } + + /** 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]; + } } - - /** - * (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); + } + + private static ObjectMapper mapper; + + /** Print JSON Parse time to the console in milliseconds */ + static boolean profileJSON = false; + + static final String sanitizeName(String name) { + if ("".equals(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; } - - /** - * (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); + 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); + } } - /** - * 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); - } + 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); + } + + /** + * 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); + } + + ///// + ///// + + 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(); + } + } + + /** + * Configures the IMU mode for MegaTag2 Localization + * + * @param limelightName Name/identifier of the Limelight + * @param mode IMU mode. + */ + public static void SetIMUMode(String limelightName, int mode) { + setLimelightNTDouble(limelightName, "imumode_set", mode); + } + + /** + * 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 && !"".equals(snapshotName)) { + connection.setRequestProperty("snapname", snapshotName); + } - /** - * 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); + 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 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 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) { - /** - * 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); + long start = System.nanoTime(); + LimelightHelpers.LimelightResults results = new LimelightHelpers.LimelightResults(); + if (mapper == null) { + mapper = + new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false); } - /** - * 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); + try { + results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); + } catch (JsonProcessingException e) { + results.error = "lljson error: " + e.getMessage(); } - /** - * 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); - - } - - /** - * 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); - } - - ///// - ///// - - 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(); - } - } - - /** - * Configures the IMU mode for MegaTag2 Localization - * - * @param limelightName Name/identifier of the Limelight - * @param mode IMU mode. - */ - public static void SetIMUMode(String limelightName, int mode) { - setLimelightNTDouble(limelightName, "imumode_set", mode); - } - - /** - * 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); - }); + long end = System.nanoTime(); + double millis = (end - start) * .000001; + results.latency_jsonParse = millis; + if (profileJSON) { + System.out.printf("lljson: %.2f\r\n", millis); } - 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 && !"".equals(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; - } -} \ No newline at end of file + return results; + } +} From be3260468bd09d31abef19d565801b59d8451f18 Mon Sep 17 00:00:00 2001 From: sub0dev <125705137+Mr-Pyro@users.noreply.github.com> Date: Thu, 15 Jan 2026 16:26:37 -0500 Subject: [PATCH 7/9] real constants --- src/main/java/frc/robot/subsystems/vision/Limelight.java | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/vision/Limelight.java b/src/main/java/frc/robot/subsystems/vision/Limelight.java index f1c46365..7c79a500 100644 --- a/src/main/java/frc/robot/subsystems/vision/Limelight.java +++ b/src/main/java/frc/robot/subsystems/vision/Limelight.java @@ -26,6 +26,9 @@ public class Limelight extends SubsystemBase { /* CONSTANTS */ + public static final double hubCenterX = 4.6245018; + public static final double hubCenterY = 4.105; + public static final double hubTagHeightMeters = 1.12395; public static final double trenchTagHeightMeters = 0.889; public static final double towerTagHeightMeters = 0.55245; @@ -277,7 +280,7 @@ public int getTagID() { public double getDistanceToHub() { Pose2d robotPose = RobotContainer.drivetrain.getRobotPose(); - Translation2d hubLocation = new Translation2d(4.6245018, 4.105); + Translation2d hubLocation = new Translation2d(hubCenterX, hubCenterY); Pose2d hubPose = new Pose2d(hubLocation, Rotation2d.fromDegrees(0)); hubPose = PoseUtils.flipPoseAlliance(hubPose); return robotPose.getTranslation().getDistance(hubPose.getTranslation()); @@ -285,7 +288,7 @@ public double getDistanceToHub() { public double getAngleToHub() { Pose2d robotPose = RobotContainer.drivetrain.getRobotPose(); - Translation2d hubLocation = new Translation2d(4.6245018, 4.105); + Translation2d hubLocation = new Translation2d(hubCenterX, hubCenterY); Pose2d hubPose = new Pose2d(hubLocation, Rotation2d.fromDegrees(0)); hubPose = PoseUtils.flipPoseAlliance(hubPose); Translation2d robotToHub = hubPose.getTranslation().minus(robotPose.getTranslation()); @@ -353,4 +356,4 @@ public Command flashLEDs() { public Command ifHasTarget(Command cmd) { return cmd.onlyWhile(this::hasValidTarget); } -} +} \ No newline at end of file From 1d207d0cc56430ccf1d54165ce584fd9b10e547f Mon Sep 17 00:00:00 2001 From: rafaelbaird <155581003+rafaelbaird@users.noreply.github.com> Date: Fri, 16 Jan 2026 21:44:52 -0500 Subject: [PATCH 8/9] Added vision2, Vision using AdvantageKit's template Went through and read every file to understand. They are doing a couple of things that are great: - better logging through io layers - logging is separate from class logic, makes everything a TON more readable - no stupid bullshit periodic code - one vision object with multiple cameras that can be anything (sim, limelight, limelight without interal imu) - --- src/main/java/frc/robot/BuildConstants.java | 10 +- src/main/java/frc/robot/RobotContainer.java | 64 +++++-- .../robot/subsystems/vision/Limelight.java | 2 +- .../frc/robot/subsystems/vision2/Vision.java | 180 ++++++++++++++++++ .../subsystems/vision2/VisionConstants.java | 55 ++++++ .../robot/subsystems/vision2/VisionIO.java | 43 +++++ .../subsystems/vision2/VisionIOLimelight.java | 152 +++++++++++++++ .../vision2/VisionIOPhotonVision.java | 125 ++++++++++++ .../vision2/VisionIOPhotonVisionSim.java | 54 ++++++ vendordeps/photonlib-v2026.1.1-rc-3.json | 71 +++++++ 10 files changed, 738 insertions(+), 18 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/vision2/Vision.java create mode 100644 src/main/java/frc/robot/subsystems/vision2/VisionConstants.java create mode 100644 src/main/java/frc/robot/subsystems/vision2/VisionIO.java create mode 100644 src/main/java/frc/robot/subsystems/vision2/VisionIOLimelight.java create mode 100644 src/main/java/frc/robot/subsystems/vision2/VisionIOPhotonVision.java create mode 100644 src/main/java/frc/robot/subsystems/vision2/VisionIOPhotonVisionSim.java create mode 100644 vendordeps/photonlib-v2026.1.1-rc-3.json diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 34e19256..c72202b9 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Rebuilt"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 7; - public static final String GIT_SHA = "2e3c5b3bb2e6565c2f2ce9754290f5c6c3190222"; - public static final String GIT_DATE = "2026-01-14 15:18:16 EST"; + public static final int GIT_REVISION = 9; + public static final String GIT_SHA = "be3260468bd09d31abef19d565801b59d8451f18"; + public static final String GIT_DATE = "2026-01-15 16:26:37 EST"; public static final String GIT_BRANCH = "vision"; - public static final String BUILD_DATE = "2026-01-14 16:26:24 EST"; - public static final long BUILD_UNIX_TIME = 1768425984934L; + public static final String BUILD_DATE = "2026-01-16 19:57:06 EST"; + public static final long BUILD_UNIX_TIME = 1768611426692L; public static final int DIRTY = 0; private BuildConstants() {} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cf663d8a..fbec8946 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,27 +4,67 @@ package frc.robot; +import static frc.robot.subsystems.vision2.VisionConstants.*; + import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandPS5Controller; +import frc.robot.constants.Constants; import frc.robot.subsystems.drivetrain.CommandSwerveDrivetrain; +import frc.robot.subsystems.vision2.Vision; +import frc.robot.subsystems.vision2.VisionIO; +import frc.robot.subsystems.vision2.VisionIOLimelight; +import frc.robot.subsystems.vision2.VisionIOPhotonVisionSim; public class RobotContainer { - // Controllers - public static CommandPS5Controller driverController = new CommandPS5Controller(0); - public static CommandPS5Controller operatorController = new CommandPS5Controller(1); + private final Vision vision; + + // Controllers + public static CommandPS5Controller driverController = new CommandPS5Controller(0); + public static CommandPS5Controller operatorController = new CommandPS5Controller(1); + + // Subsystems + public static CommandSwerveDrivetrain drivetrain; + + public RobotContainer() { + switch (Constants.currentMode) { + case REAL: + // Real robot, instantiate hardware IO implementations + vision = + new Vision( + drivetrain::addVisionMeasurement, + new VisionIOLimelight(camera0Name, () -> drivetrain.odometryHeading), + new VisionIOLimelight(camera1Name, () -> drivetrain.odometryHeading)); + + break; - // Subsystems - public static CommandSwerveDrivetrain drivetrain; + case SIM: + // Sim robot, instantiate physics sim IO implementations + vision = + new Vision( + drivetrain::addVisionMeasurement, + new VisionIOPhotonVisionSim("camera0Name", robotToCamera0, drivetrain::getRobotPose), + new VisionIOPhotonVisionSim("camera1Name", robotToCamera1, drivetrain::getRobotPose)); + break; - public RobotContainer() { - configureBindings(); - } + default: + // Replayed robot, disable IO implementations + // (Use same number of dummy implementations as the real robot) + vision = + new Vision( + drivetrain::addVisionMeasurement, + new VisionIO() {}, + new VisionIO() {}); + break; + } + + configureBindings(); + } - private void configureBindings() {} + private void configureBindings() {} - public Command getAutonomousCommand() { - return Commands.print("No autonomous command configured"); - } + public Command getAutonomousCommand() { + return Commands.print("No autonomous command configured"); + } } diff --git a/src/main/java/frc/robot/subsystems/vision/Limelight.java b/src/main/java/frc/robot/subsystems/vision/Limelight.java index 7c79a500..503bab41 100644 --- a/src/main/java/frc/robot/subsystems/vision/Limelight.java +++ b/src/main/java/frc/robot/subsystems/vision/Limelight.java @@ -356,4 +356,4 @@ public Command flashLEDs() { public Command ifHasTarget(Command cmd) { return cmd.onlyWhile(this::hasValidTarget); } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/vision2/Vision.java b/src/main/java/frc/robot/subsystems/vision2/Vision.java new file mode 100644 index 00000000..127e2808 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision2/Vision.java @@ -0,0 +1,180 @@ +// Copyright (c) 2021-2026 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by a BSD +// license that can be found in the LICENSE file +// at the root directory of this project. + +package frc.robot.subsystems.vision2; + +import static frc.robot.subsystems.vision2.VisionConstants.*; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +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.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.vision2.VisionIO.PoseObservationType; +import java.util.LinkedList; +import java.util.List; +import org.littletonrobotics.junction.Logger; + +public class Vision extends SubsystemBase { + private final VisionConsumer consumer; //lamda expression that takes in values and records a vision measurement + private final VisionIO[] io; + private final VisionIOInputsAutoLogged[] inputs; + private final Alert[] disconnectedAlerts; + + public Vision(VisionConsumer consumer, VisionIO... io) { + this.consumer = consumer; + this.io = io; + + //for loops serve the purpose of intitializing inputs for all cameras, as the ios are stored in an array + // Initialize inputs + this.inputs = new VisionIOInputsAutoLogged[io.length]; + for (int i = 0; i < inputs.length; i++) { + inputs[i] = new VisionIOInputsAutoLogged(); + } + + // Initialize disconnected alerts + this.disconnectedAlerts = new Alert[io.length]; + for (int i = 0; i < inputs.length; i++) { + disconnectedAlerts[i] = + new Alert( + "Vision camera " + Integer.toString(i) + " is disconnected.", AlertType.kWarning); + } + } + + /** + * Returns the X angle to the best target, which can be used for simple servoing with vision. + * + * @param cameraIndex The index of the camera to use. + */ + public Rotation2d getTargetX(int cameraIndex) { + return inputs[cameraIndex].latestTargetObservation.tx(); + } + + @Override + public void periodic() { + for (int i = 0; i < io.length; i++) { + io[i].updateInputs(inputs[i]); + Logger.processInputs("Vision/Camera" + Integer.toString(i), inputs[i]); + } + + // Initialize logging values + List allTagPoses = new LinkedList<>(); + List allRobotPoses = new LinkedList<>(); + List allRobotPosesAccepted = new LinkedList<>(); + List allRobotPosesRejected = new LinkedList<>(); + + // Loop over cameras + for (int cameraIndex = 0; cameraIndex < io.length; cameraIndex++) { + // Update disconnected alert + disconnectedAlerts[cameraIndex].set(!inputs[cameraIndex].connected); + + // Initialize logging values + List tagPoses = new LinkedList<>(); + List robotPoses = new LinkedList<>(); + List robotPosesAccepted = new LinkedList<>(); + List robotPosesRejected = new LinkedList<>(); + + // Add tag poses + for (int tagId : inputs[cameraIndex].tagIds) { + var tagPose = aprilTagLayout.getTagPose(tagId); + if (tagPose.isPresent()) { + tagPoses.add(tagPose.get()); + } + } + + // Loop over pose observations + for (var observation : inputs[cameraIndex].poseObservations) { + // Check whether to reject pose + boolean rejectPose = + observation.tagCount() == 0 // Must have at least one tag + || (observation.tagCount() == 1 + //ambiguity is 0 for megatag2 + && observation.ambiguity() > maxAmbiguity) // Cannot be high ambiguity + || Math.abs(observation.pose().getZ()) + > maxZError // Must have realistic Z coordinate + + // Must be within the field boundaries + || observation.pose().getX() < 0.0 + || observation.pose().getX() > aprilTagLayout.getFieldLength() + || observation.pose().getY() < 0.0 + || observation.pose().getY() > aprilTagLayout.getFieldWidth(); + + // Add pose to log + robotPoses.add(observation.pose()); + if (rejectPose) { + robotPosesRejected.add(observation.pose()); + } else { + robotPosesAccepted.add(observation.pose()); + } + + // Skip if rejected + if (rejectPose) { + continue; + } + + // Calculate standard deviations + double stdDevFactor = + Math.pow(observation.averageTagDistance(), 2.0) / observation.tagCount(); + double linearStdDev = linearStdDevBaseline * stdDevFactor; + double angularStdDev = angularStdDevBaseline * stdDevFactor; + if (observation.type() == PoseObservationType.MEGATAG_2) { + linearStdDev *= linearStdDevMegatag2Factor; + angularStdDev *= angularStdDevMegatag2Factor; + } + if (cameraIndex < cameraStdDevFactors.length) { + linearStdDev *= cameraStdDevFactors[cameraIndex]; + angularStdDev *= cameraStdDevFactors[cameraIndex]; + } + + // Send vision observation + consumer.accept( + observation.pose().toPose2d(), + observation.timestamp(), + VecBuilder.fill(linearStdDev, linearStdDev, angularStdDev)); + } + + // Log camera metadata + Logger.recordOutput( + "Vision/Camera" + Integer.toString(cameraIndex) + "/TagPoses", + tagPoses.toArray(new Pose3d[0])); + Logger.recordOutput( + "Vision/Camera" + Integer.toString(cameraIndex) + "/RobotPoses", + robotPoses.toArray(new Pose3d[0])); + Logger.recordOutput( + "Vision/Camera" + Integer.toString(cameraIndex) + "/RobotPosesAccepted", + robotPosesAccepted.toArray(new Pose3d[0])); + Logger.recordOutput( + "Vision/Camera" + Integer.toString(cameraIndex) + "/RobotPosesRejected", + robotPosesRejected.toArray(new Pose3d[0])); + allTagPoses.addAll(tagPoses); + allRobotPoses.addAll(robotPoses); + allRobotPosesAccepted.addAll(robotPosesAccepted); + allRobotPosesRejected.addAll(robotPosesRejected); + } + + // Log summary data + Logger.recordOutput("Vision/Summary/TagPoses", allTagPoses.toArray(new Pose3d[0])); + Logger.recordOutput("Vision/Summary/RobotPoses", allRobotPoses.toArray(new Pose3d[0])); + Logger.recordOutput( + "Vision/Summary/RobotPosesAccepted", allRobotPosesAccepted.toArray(new Pose3d[0])); + Logger.recordOutput( + "Vision/Summary/RobotPosesRejected", allRobotPosesRejected.toArray(new Pose3d[0])); + } + + @FunctionalInterface + public static interface VisionConsumer { + public void accept( + Pose2d visionRobotPoseMeters, + double timestampSeconds, + Matrix visionMeasurementStdDevs); + } +} diff --git a/src/main/java/frc/robot/subsystems/vision2/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision2/VisionConstants.java new file mode 100644 index 00000000..d337cb71 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision2/VisionConstants.java @@ -0,0 +1,55 @@ +// Copyright (c) 2021-2026 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by a BSD +// license that can be found in the LICENSE file +// at the root directory of this project. + +package frc.robot.subsystems.vision2; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; + +public class VisionConstants { + // AprilTag layout + public static AprilTagFieldLayout aprilTagLayout = + AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); + + // Camera names, must match names configured on coprocessor + //TODO: name these better + public static String camera0Name = "camera_0"; + public static String camera1Name = "camera_1"; + + // Robot to camera transforms + // (Not used by Limelight, configure in web UI instead) + public static Transform3d robotToCamera0 = + new Transform3d(0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, 0.0)); + public static Transform3d robotToCamera1 = + new Transform3d(-0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, Math.PI)); + + // Basic filtering thresholds + public static double maxAmbiguity = 0.3; + public static double maxZError = 0.75; + + // Standard deviation baselines, for 1 meter distance and 1 tag + // (Adjusted automatically based on distance and # of tags) + public static double linearStdDevBaseline = 0.02; // Meters + public static double angularStdDevBaseline = 0.06; // Radians + + // Standard deviation multipliers for each camera + // (Adjust to trust some cameras more than others) + // value greater than one, trust cameras less, value less than one, trust cameras more + // TODO: check the above statement ^^ + public static double[] cameraStdDevFactors = + new double[] { + 1.0, // Camera 0 + 1.0 // Camera 1 + }; + + // Multipliers to apply for MegaTag 2 observations + public static double linearStdDevMegatag2Factor = 0.5; // More stable than full 3D solve + public static double angularStdDevMegatag2Factor = + Double.POSITIVE_INFINITY; // No rotation data available +} diff --git a/src/main/java/frc/robot/subsystems/vision2/VisionIO.java b/src/main/java/frc/robot/subsystems/vision2/VisionIO.java new file mode 100644 index 00000000..9dfb30af --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision2/VisionIO.java @@ -0,0 +1,43 @@ +// Copyright (c) 2021-2026 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by a BSD +// license that can be found in the LICENSE file +// at the root directory of this project. + +package frc.robot.subsystems.vision2; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.AutoLog; + +public interface VisionIO { + @AutoLog + public static class VisionIOInputs { + public boolean connected = false; + public TargetObservation latestTargetObservation = + new TargetObservation(Rotation2d.kZero, Rotation2d.kZero); + public PoseObservation[] poseObservations = new PoseObservation[0]; + public int[] tagIds = new int[0]; + } + + /** Represents the angle to a simple target, not used for pose estimation. */ + public static record TargetObservation(Rotation2d tx, Rotation2d ty) {} + + /** Represents a robot pose sample used for pose estimation. */ + public static record PoseObservation( + double timestamp, + Pose3d pose, + double ambiguity, + int tagCount, + double averageTagDistance, + PoseObservationType type) {} + + public static enum PoseObservationType { + MEGATAG_1, + MEGATAG_2, + PHOTONVISION + } + + public default void updateInputs(VisionIOInputs inputs) {} +} diff --git a/src/main/java/frc/robot/subsystems/vision2/VisionIOLimelight.java b/src/main/java/frc/robot/subsystems/vision2/VisionIOLimelight.java new file mode 100644 index 00000000..7914e5d5 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision2/VisionIOLimelight.java @@ -0,0 +1,152 @@ +// Copyright (c) 2021-2026 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by a BSD +// license that can be found in the LICENSE file +// at the root directory of this project. + +package frc.robot.subsystems.vision2; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.networktables.DoubleArrayPublisher; +import edu.wpi.first.networktables.DoubleArraySubscriber; +import edu.wpi.first.networktables.DoubleSubscriber; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.RobotController; +import java.util.HashSet; +import java.util.LinkedList; +import java.util.List; +import java.util.Set; +import java.util.function.Supplier; + +/** IO implementation for real Limelight hardware. */ +public class VisionIOLimelight implements VisionIO { + private final Supplier rotationSupplier; + private final DoubleArrayPublisher orientationPublisher; + + private final DoubleSubscriber latencySubscriber; + private final DoubleSubscriber txSubscriber; + private final DoubleSubscriber tySubscriber; + private final DoubleArraySubscriber megatag1Subscriber; + private final DoubleArraySubscriber megatag2Subscriber; + + /** + * Creates a new VisionIOLimelight. + * + * @param name The configured name of the Limelight. + * @param rotationSupplier Supplier for the current estimated rotation, used for MegaTag 2. + */ + public VisionIOLimelight(String name, Supplier rotationSupplier) { + var table = NetworkTableInstance.getDefault().getTable(name); + this.rotationSupplier = rotationSupplier; + orientationPublisher = table.getDoubleArrayTopic("robot_orientation_set").publish(); + latencySubscriber = table.getDoubleTopic("tl").subscribe(0.0); + txSubscriber = table.getDoubleTopic("tx").subscribe(0.0); + tySubscriber = table.getDoubleTopic("ty").subscribe(0.0); + megatag1Subscriber = table.getDoubleArrayTopic("botpose_wpiblue").subscribe(new double[] {}); + megatag2Subscriber = + table.getDoubleArrayTopic("botpose_orb_wpiblue").subscribe(new double[] {}); + } + + @Override + public void updateInputs(VisionIOInputs inputs) { + // Update connection status based on whether an update has been seen in the last + // 250ms + inputs.connected = + ((RobotController.getFPGATime() - latencySubscriber.getLastChange()) / 1000) < 250; + + // Update target observation + inputs.latestTargetObservation = + new TargetObservation( + Rotation2d.fromDegrees(txSubscriber.get()), Rotation2d.fromDegrees(tySubscriber.get())); + + // Update orientation for MegaTag 2 + orientationPublisher.accept( + new double[] {rotationSupplier.get().getDegrees(), 0.0, 0.0, 0.0, 0.0, 0.0}); + NetworkTableInstance.getDefault() + .flush(); // Increases network traffic but recommended by Limelight + + // Read new pose observations from NetworkTables + Set tagIds = new HashSet<>(); + List poseObservations = new LinkedList<>(); + for (var rawSample : megatag1Subscriber.readQueue()) { + if (rawSample.value.length == 0) continue; + for (int i = 11; i < rawSample.value.length; i += 7) { + tagIds.add((int) rawSample.value[i]); + } + poseObservations.add( + new PoseObservation( + // Timestamp, based on server timestamp of publish and latency + rawSample.timestamp * 1.0e-6 - rawSample.value[6] * 1.0e-3, + + // 3D pose estimate + parsePose(rawSample.value), + + // Ambiguity, using only the first tag because ambiguity isn't applicable for + // multitag + rawSample.value.length >= 18 ? rawSample.value[17] : 0.0, + + // Tag count + (int) rawSample.value[7], + + // Average tag distance + rawSample.value[9], + + // Observation type + PoseObservationType.MEGATAG_1)); + } + for (var rawSample : megatag2Subscriber.readQueue()) { + if (rawSample.value.length == 0) continue; + for (int i = 11; i < rawSample.value.length; i += 7) { + tagIds.add((int) rawSample.value[i]); + } + poseObservations.add( + new PoseObservation( + // Timestamp, based on server timestamp of publish and latency + rawSample.timestamp * 1.0e-6 - rawSample.value[6] * 1.0e-3, + + // 3D pose estimate + parsePose(rawSample.value), + + // Ambiguity, zeroed because the pose is already disambiguated + 0.0, + + // Tag count + (int) rawSample.value[7], + + // Average tag distance + rawSample.value[9], + + // Observation type + PoseObservationType.MEGATAG_2)); + } + + // Save pose observations to inputs object + inputs.poseObservations = new PoseObservation[poseObservations.size()]; + for (int i = 0; i < poseObservations.size(); i++) { + inputs.poseObservations[i] = poseObservations.get(i); + } + + // Save tag IDs to inputs objects + inputs.tagIds = new int[tagIds.size()]; + int i = 0; + for (int id : tagIds) { + inputs.tagIds[i++] = id; + } + } + + /** Parses the 3D pose from a Limelight botpose array. */ + private static Pose3d parsePose(double[] rawLLArray) { + return new Pose3d( + rawLLArray[0], + rawLLArray[1], + rawLLArray[2], + new Rotation3d( + Units.degreesToRadians(rawLLArray[3]), + Units.degreesToRadians(rawLLArray[4]), + Units.degreesToRadians(rawLLArray[5]))); + } +} diff --git a/src/main/java/frc/robot/subsystems/vision2/VisionIOPhotonVision.java b/src/main/java/frc/robot/subsystems/vision2/VisionIOPhotonVision.java new file mode 100644 index 00000000..bd2d800a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision2/VisionIOPhotonVision.java @@ -0,0 +1,125 @@ +// Copyright (c) 2021-2026 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by a BSD +// license that can be found in the LICENSE file +// at the root directory of this project. + +package frc.robot.subsystems.vision2; + +import static frc.robot.subsystems.vision2.VisionConstants.*; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform3d; +import java.util.HashSet; +import java.util.LinkedList; +import java.util.List; +import java.util.Set; +import org.photonvision.PhotonCamera; + +/** IO implementation for real PhotonVision hardware. */ +public class VisionIOPhotonVision implements VisionIO { + protected final PhotonCamera camera; + protected final Transform3d robotToCamera; + + /** + * Creates a new VisionIOPhotonVision. + * + * @param name The configured name of the camera. + * @param robotToCamera The 3D position of the camera relative to the robot. + */ + public VisionIOPhotonVision(String name, Transform3d robotToCamera) { + camera = new PhotonCamera(name); + this.robotToCamera = robotToCamera; + } + + @Override + public void updateInputs(VisionIOInputs inputs) { + inputs.connected = camera.isConnected(); + + // Read new camera observations + Set tagIds = new HashSet<>(); + List poseObservations = new LinkedList<>(); + for (var result : camera.getAllUnreadResults()) { + // Update latest target observation + if (result.hasTargets()) { + inputs.latestTargetObservation = + new TargetObservation( + Rotation2d.fromDegrees(result.getBestTarget().getYaw()), + Rotation2d.fromDegrees(result.getBestTarget().getPitch())); + } else { + inputs.latestTargetObservation = new TargetObservation(Rotation2d.kZero, Rotation2d.kZero); + } + + // Add pose observation + if (result.multitagResult.isPresent()) { // Multitag result + var multitagResult = result.multitagResult.get(); + + // Calculate robot pose + Transform3d fieldToCamera = multitagResult.estimatedPose.best; + Transform3d fieldToRobot = fieldToCamera.plus(robotToCamera.inverse()); + Pose3d robotPose = new Pose3d(fieldToRobot.getTranslation(), fieldToRobot.getRotation()); + + // Calculate average tag distance + double totalTagDistance = 0.0; + for (var target : result.targets) { + totalTagDistance += target.bestCameraToTarget.getTranslation().getNorm(); + } + + // Add tag IDs + tagIds.addAll(multitagResult.fiducialIDsUsed); + + // Add observation + poseObservations.add( + new PoseObservation( + result.getTimestampSeconds(), // Timestamp + robotPose, // 3D pose estimate + multitagResult.estimatedPose.ambiguity, // Ambiguity + multitagResult.fiducialIDsUsed.size(), // Tag count + totalTagDistance / result.targets.size(), // Average tag distance + PoseObservationType.PHOTONVISION)); // Observation type + + } else if (!result.targets.isEmpty()) { // Single tag result + var target = result.targets.get(0); + + // Calculate robot pose + var tagPose = aprilTagLayout.getTagPose(target.fiducialId); + if (tagPose.isPresent()) { + Transform3d fieldToTarget = + new Transform3d(tagPose.get().getTranslation(), tagPose.get().getRotation()); + Transform3d cameraToTarget = target.bestCameraToTarget; + Transform3d fieldToCamera = fieldToTarget.plus(cameraToTarget.inverse()); + Transform3d fieldToRobot = fieldToCamera.plus(robotToCamera.inverse()); + Pose3d robotPose = new Pose3d(fieldToRobot.getTranslation(), fieldToRobot.getRotation()); + + // Add tag ID + tagIds.add((short) target.fiducialId); + + // Add observation + poseObservations.add( + new PoseObservation( + result.getTimestampSeconds(), // Timestamp + robotPose, // 3D pose estimate + target.poseAmbiguity, // Ambiguity + 1, // Tag count + cameraToTarget.getTranslation().getNorm(), // Average tag distance + PoseObservationType.PHOTONVISION)); // Observation type + } + } + } + + // Save pose observations to inputs object + inputs.poseObservations = new PoseObservation[poseObservations.size()]; + for (int i = 0; i < poseObservations.size(); i++) { + inputs.poseObservations[i] = poseObservations.get(i); + } + + // Save tag IDs to inputs objects + inputs.tagIds = new int[tagIds.size()]; + int i = 0; + for (int id : tagIds) { + inputs.tagIds[i++] = id; + } + } +} diff --git a/src/main/java/frc/robot/subsystems/vision2/VisionIOPhotonVisionSim.java b/src/main/java/frc/robot/subsystems/vision2/VisionIOPhotonVisionSim.java new file mode 100644 index 00000000..4d0f61eb --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision2/VisionIOPhotonVisionSim.java @@ -0,0 +1,54 @@ +// Copyright (c) 2021-2026 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by a BSD +// license that can be found in the LICENSE file +// at the root directory of this project. + +package frc.robot.subsystems.vision2; + +import static frc.robot.subsystems.vision2.VisionConstants.aprilTagLayout; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Transform3d; +import java.util.function.Supplier; +import org.photonvision.simulation.PhotonCameraSim; +import org.photonvision.simulation.SimCameraProperties; +import org.photonvision.simulation.VisionSystemSim; + +/** IO implementation for physics sim using PhotonVision simulator. */ +public class VisionIOPhotonVisionSim extends VisionIOPhotonVision { + private static VisionSystemSim visionSim; + + private final Supplier poseSupplier; + private final PhotonCameraSim cameraSim; + + /** + * Creates a new VisionIOPhotonVisionSim. + * + * @param name The name of the camera. + * @param poseSupplier Supplier for the robot pose to use in simulation. + */ + public VisionIOPhotonVisionSim( + String name, Transform3d robotToCamera, Supplier poseSupplier) { + super(name, robotToCamera); + this.poseSupplier = poseSupplier; + + // Initialize vision sim + if (visionSim == null) { + visionSim = new VisionSystemSim("main"); + visionSim.addAprilTags(aprilTagLayout); + } + + // Add sim camera + var cameraProperties = new SimCameraProperties(); + cameraSim = new PhotonCameraSim(camera, cameraProperties, aprilTagLayout); + visionSim.addCamera(cameraSim, robotToCamera); + } + + @Override + public void updateInputs(VisionIOInputs inputs) { + visionSim.update(poseSupplier.get()); + super.updateInputs(inputs); + } +} diff --git a/vendordeps/photonlib-v2026.1.1-rc-3.json b/vendordeps/photonlib-v2026.1.1-rc-3.json new file mode 100644 index 00000000..75084811 --- /dev/null +++ b/vendordeps/photonlib-v2026.1.1-rc-3.json @@ -0,0 +1,71 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2026.1.1-rc-3", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", + "frcYear": "2026", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", + "jniDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2026.1.1-rc-3", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-cpp", + "version": "v2026.1.1-rc-3", + "libName": "photonlib", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2026.1.1-rc-3", + "libName": "photontargeting", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-java", + "version": "v2026.1.1-rc-3" + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-java", + "version": "v2026.1.1-rc-3" + } + ] +} From d3999216d654665a21c8865487c22f44768f8ec6 Mon Sep 17 00:00:00 2001 From: rafaelbaird <155581003+rafaelbaird@users.noreply.github.com> Date: Wed, 21 Jan 2026 21:51:39 -0500 Subject: [PATCH 9/9] Removed old vision folder for merge into main --- src/main/java/frc/robot/BuildConstants.java | 12 +- src/main/java/frc/robot/RobotContainer.java | 96 +- .../robot/subsystems/vision/Limelight.java | 359 ---- .../subsystems/vision/LimelightHelpers.java | 1692 ----------------- .../{vision2 => vision}/Vision.java | 7 +- .../{vision2 => vision}/VisionConstants.java | 4 +- .../{vision2 => vision}/VisionIO.java | 2 +- .../VisionIOLimelight.java | 2 +- .../VisionIOPhotonVision.java | 4 +- .../VisionIOPhotonVisionSim.java | 4 +- 10 files changed, 65 insertions(+), 2117 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/vision/Limelight.java delete mode 100644 src/main/java/frc/robot/subsystems/vision/LimelightHelpers.java rename src/main/java/frc/robot/subsystems/{vision2 => vision}/Vision.java (97%) rename src/main/java/frc/robot/subsystems/{vision2 => vision}/VisionConstants.java (96%) rename src/main/java/frc/robot/subsystems/{vision2 => vision}/VisionIO.java (97%) rename src/main/java/frc/robot/subsystems/{vision2 => vision}/VisionIOLimelight.java (99%) rename src/main/java/frc/robot/subsystems/{vision2 => vision}/VisionIOPhotonVision.java (97%) rename src/main/java/frc/robot/subsystems/{vision2 => vision}/VisionIOPhotonVisionSim.java (93%) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index c72202b9..1da2ffc9 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,13 +5,13 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Rebuilt"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 9; - public static final String GIT_SHA = "be3260468bd09d31abef19d565801b59d8451f18"; - public static final String GIT_DATE = "2026-01-15 16:26:37 EST"; + public static final int GIT_REVISION = 10; + public static final String GIT_SHA = "1d207d0cc56430ccf1d54165ce584fd9b10e547f"; + public static final String GIT_DATE = "2026-01-16 21:44:52 EST"; public static final String GIT_BRANCH = "vision"; - public static final String BUILD_DATE = "2026-01-16 19:57:06 EST"; - public static final long BUILD_UNIX_TIME = 1768611426692L; - public static final int DIRTY = 0; + public static final String BUILD_DATE = "2026-01-21 21:48:49 EST"; + public static final long BUILD_UNIX_TIME = 1769050129489L; + public static final int DIRTY = 1; private BuildConstants() {} } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fbec8946..9154609c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,67 +4,67 @@ package frc.robot; -import static frc.robot.subsystems.vision2.VisionConstants.*; +import static frc.robot.subsystems.vision.VisionConstants.*; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandPS5Controller; import frc.robot.constants.Constants; import frc.robot.subsystems.drivetrain.CommandSwerveDrivetrain; -import frc.robot.subsystems.vision2.Vision; -import frc.robot.subsystems.vision2.VisionIO; -import frc.robot.subsystems.vision2.VisionIOLimelight; -import frc.robot.subsystems.vision2.VisionIOPhotonVisionSim; +import frc.robot.subsystems.vision.Vision; +import frc.robot.subsystems.vision.VisionIO; +import frc.robot.subsystems.vision.VisionIOLimelight; +import frc.robot.subsystems.vision.VisionIOPhotonVisionSim; public class RobotContainer { - private final Vision vision; - - // Controllers - public static CommandPS5Controller driverController = new CommandPS5Controller(0); - public static CommandPS5Controller operatorController = new CommandPS5Controller(1); + private final Vision vision; - // Subsystems - public static CommandSwerveDrivetrain drivetrain; + // Controllers + public static CommandPS5Controller driverController = new CommandPS5Controller(0); + public static CommandPS5Controller operatorController = new CommandPS5Controller(1); - public RobotContainer() { - switch (Constants.currentMode) { - case REAL: - // Real robot, instantiate hardware IO implementations - vision = - new Vision( - drivetrain::addVisionMeasurement, - new VisionIOLimelight(camera0Name, () -> drivetrain.odometryHeading), - new VisionIOLimelight(camera1Name, () -> drivetrain.odometryHeading)); - - break; + // Subsystems + public static CommandSwerveDrivetrain drivetrain; - case SIM: - // Sim robot, instantiate physics sim IO implementations - vision = - new Vision( - drivetrain::addVisionMeasurement, - new VisionIOPhotonVisionSim("camera0Name", robotToCamera0, drivetrain::getRobotPose), - new VisionIOPhotonVisionSim("camera1Name", robotToCamera1, drivetrain::getRobotPose)); - break; + public RobotContainer() { + switch (Constants.currentMode) { + case REAL: + // Real robot, instantiate hardware IO implementations + vision = + new Vision( + drivetrain::addVisionMeasurement, + new VisionIOLimelight(camera0Name, () -> drivetrain.odometryHeading), + new VisionIOLimelight(camera1Name, () -> drivetrain.odometryHeading)); - default: - // Replayed robot, disable IO implementations - // (Use same number of dummy implementations as the real robot) - vision = - new Vision( - drivetrain::addVisionMeasurement, - new VisionIO() {}, - new VisionIO() {}); - break; - } - - configureBindings(); - } + break; - private void configureBindings() {} + case SIM: + // Sim robot, instantiate physics sim IO implementations + vision = + new Vision( + drivetrain::addVisionMeasurement, + new VisionIOPhotonVisionSim("camera0Name", robotToCamera0, drivetrain::getRobotPose), + new VisionIOPhotonVisionSim("camera1Name", robotToCamera1, drivetrain::getRobotPose)); + break; - public Command getAutonomousCommand() { - return Commands.print("No autonomous command configured"); - } + default: + // Replayed robot, disable IO implementations + // (Use same number of dummy implementations as the real robot) + vision = + new Vision( + drivetrain::addVisionMeasurement, + new VisionIO() {}, + new VisionIO() {}); + break; + } + + configureBindings(); + } + + private void configureBindings() {} + + public Command getAutonomousCommand() { + return Commands.print("No autonomous command configured"); + } } diff --git a/src/main/java/frc/robot/subsystems/vision/Limelight.java b/src/main/java/frc/robot/subsystems/vision/Limelight.java deleted file mode 100644 index 503bab41..00000000 --- a/src/main/java/frc/robot/subsystems/vision/Limelight.java +++ /dev/null @@ -1,359 +0,0 @@ -package frc.robot.subsystems.vision; - -import com.ctre.phoenix6.Utils; -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.filter.Debouncer; -import edu.wpi.first.math.filter.Debouncer.DebounceType; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.networktables.DoublePublisher; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.RobotContainer; -import frc.robot.subsystems.vision.LimelightHelpers.RawFiducial; -import frc.robot.util.PoseUtils; -import java.util.Optional; -import java.util.function.DoubleSupplier; -import org.littletonrobotics.junction.AutoLogOutput; -import org.littletonrobotics.junction.Logger; - -public class Limelight extends SubsystemBase { - - /* CONSTANTS */ - public static final double hubCenterX = 4.6245018; - public static final double hubCenterY = 4.105; - - public static final double hubTagHeightMeters = 1.12395; - public static final double trenchTagHeightMeters = 0.889; - public static final double towerTagHeightMeters = 0.55245; - public static final double outpostTagHeightMeters = 0.55245; - - public static final int[] hubIDsRed = {2, 3, 4, 5, 8, 9, 10, 11}; - public static final int[] hubIDsBlue = {18, 19, 20, 21, 24, 25, 26, 27}; - public static final int[] reefIDs = {2, 3, 4, 5, 8, 9, 10, 11, 18, 19, 20, 21, 24, 25, 26, 27}; - - public static final int[] towerIDsRed = {15, 16}; - public static final int[] towerIDsBlue = {31, 32}; - public static final int[] towerStationIDs = {15, 16, 31, 32}; - - public static final int[] trenchIDsRed = {1, 6, 7, 12}; - public static final int[] trenchIDsBlue = {17, 22, 23, 28}; - public static final int[] trenchStationIDs = {1, 6, 7, 12, 17, 22, 23, 28}; - - public static final int[] outpostIDsRed = {15, 16}; - public static final int[] outpostIDsBlue = {31, 32}; - public static final int[] outpostStationIDs = {15, 16, 31, 32}; - - public static final double TARGET_DEBOUNCE_TIME = 0.2; - - /* INSTANCE VARIABLES */ - private int tagCount; - private int[] validIDs = {}; // TODO: set these - public String cameraName; - private double tx; - private double ty; - private Debouncer targetDebouncer = new Debouncer(TARGET_DEBOUNCE_TIME, DebounceType.kFalling); - - public static final double angleVelocityTolerance = 360 * Math.PI / 180; // in radians per sec - - private double cameraHeightMeters; - public double cameraAngle; - public double cameraOffsetX; // right is positive - public double cameraOffsetY; // forward is positive - private double angleMult; - - private boolean hasTipped; - - private DoublePublisher yDistPub; - private DoublePublisher xDistPub; - private DoublePublisher horizontalDistPub; - - // TODO setup camera IPs? - // https://docs.limelightvision.io/docs/docs-limelight/getting-started/FRC/best-practices - public Limelight( - String cameraName, - double cameraHeightMeters, - double cameraAngle, - double cameraOffsetX, - double cameraOffsetY, - boolean cameraUpsideDown) { - this.cameraName = cameraName; - this.cameraHeightMeters = cameraHeightMeters; - this.cameraAngle = cameraAngle; - this.cameraOffsetX = cameraOffsetX; - this.cameraOffsetY = cameraOffsetY; - LimelightHelpers.SetFiducialIDFiltersOverride(cameraName, validIDs); - if (cameraUpsideDown) { - angleMult = -1; - } else { - angleMult = 1; - } - - NetworkTableInstance inst = NetworkTableInstance.getDefault(); - NetworkTable lightTable = inst.getTable(cameraName); - - yDistPub = lightTable.getDoubleTopic("Y Distance").publish(); - xDistPub = lightTable.getDoubleTopic("X Distance").publish(); - horizontalDistPub = lightTable.getDoubleTopic("Horizontal Distance").publish(); - } - - // from last years robot - public double getTimestampSeconds() { - double latency = - (LimelightHelpers.getLimelightNTDouble(cameraName, "cl") - + LimelightHelpers.getLimelightNTDouble(cameraName, "tl")) - / 1000.0; - - return Timer.getFPGATimestamp() - latency; - } - - // from last years robot as well - public boolean hasValidTarget() { - boolean hasMatch = (LimelightHelpers.getLimelightNTDouble(cameraName, "tv") == 1.0); - return targetDebouncer.calculate(hasMatch); - } - - public void setGyroMode(int mode) { - LimelightHelpers.SetIMUMode(cameraName, mode); - } - - public RawFiducial getClosestTag() { - RawFiducial[] tags = LimelightHelpers.getRawFiducials(cameraName); - if (tags.length == 0) { - return null; - } - RawFiducial largest = tags[0]; - for (RawFiducial tag : tags) { - if (tag.distToRobot > largest.distToRobot) { - largest = tag; - } - } - return largest; - } - - public void poseEstimationMegatag2() { - - double angle = (RobotContainer.drivetrain.getWrappedHeading().getDegrees() + 360) % 360; - LimelightHelpers.SetRobotOrientation(cameraName, angle, 0, 0, 0, 0, 0); - LimelightHelpers.PoseEstimate mt2 = - LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(cameraName); - - boolean shouldRejectUpdate = false; - - int rejectReason = 0; - - if (mt2 != null) { - Optional optPastRobotPose = - RobotContainer.drivetrain.getPoseAtTime(mt2.timestampSeconds); - if (optPastRobotPose.isPresent()) { - Logger.recordOutput(cameraName + "/PastRobotPose", optPastRobotPose.get()); - } - Pose2d pastRobotPose = RobotContainer.drivetrain.getRobotPose(); - // Pose2d pastRobotPose = optPastRobotPose.orElseGet(() -> - // RobotContainer.drivetrain.getRobotPose()); - Logger.recordOutput(cameraName + "/timestampSeconds", mt2.timestampSeconds); - RawFiducial[] tags = mt2.rawFiducials; - int[] ids = new int[tags.length]; - for (int i = 0; i < tags.length; i++) { - ids[i] = tags[i].id; - } - Logger.recordOutput(cameraName + "/SeenTags", ids); - Logger.recordOutput( - cameraName + "/PoseLatency", mt2.timestampSeconds - Timer.getFPGATimestamp()); - if (mt2.tagCount == 0) { - // rejects current measurement if there are no aprilTags - shouldRejectUpdate = true; - rejectReason = 1; - } - if (Math.abs(RobotContainer.drivetrain.getCurrentSpeeds().omegaRadiansPerSecond) - > angleVelocityTolerance) { - shouldRejectUpdate = true; - rejectReason = 2; - } - if ((mt2.pose.getTranslation().getDistance(pastRobotPose.getTranslation()) > 0.9 - && !DriverStation.isDisabled() - && !DriverStation.isTeleopEnabled())) { - shouldRejectUpdate = true; - rejectReason = 3; - } - if (Math.abs( - PoseUtils.wrapRotation(mt2.pose.getRotation()) - .minus(PoseUtils.wrapRotation(pastRobotPose.getRotation())) - .getDegrees()) - > 3) { - shouldRejectUpdate = true; - rejectReason = 4; - } - if (mt2.avgTagDist > 4) { - shouldRejectUpdate = true; - rejectReason = 5; - } - - // adds vision measurement if conditions are met - if (!shouldRejectUpdate) { - Logger.recordOutput(cameraName + "/mt2Pose", mt2.pose); - Logger.recordOutput( - cameraName + "/Calculated stdevs", Math.pow(0.5, mt2.tagCount) * 2 * mt2.avgTagDist); - // Vector = VecBuilder.fill - RobotContainer.drivetrain.addVisionMeasurement( - mt2.pose, - Utils.fpgaToCurrentTime(mt2.timestampSeconds), - // VecBuilder.fill(0.000716, 0.0003, Double.POSITIVE_INFINITY)); - VecBuilder.fill( - Math.pow(0.5, mt2.tagCount) * 2 * mt2.avgTagDist, - Math.pow(0.5, mt2.tagCount) * 2 * mt2.avgTagDist, - Double.POSITIVE_INFINITY)); - } else { - Logger.recordOutput(cameraName + "/mt2PoseRejected", mt2.pose); - Logger.recordOutput(cameraName + "/rejectReason", rejectReason); - } - } - } - - // TODO: Do we need these / check if the trig is right - - public double getDistanceToTag(double tagHeightMeters) { - if (hasValidTarget()) { - double distance = getStraightDistanceToTag(tagHeightMeters) - cameraOffsetY; - return distance / Math.cos((Math.PI / 180.0) * getTX()); - } - return 0; - } - - public double getStraightDistanceToTag(double tagHeightMeters) { - if (hasValidTarget()) { - double distance = - (tagHeightMeters - cameraHeightMeters) - / Math.tan((Math.PI / 180.0) * (cameraAngle + getTY())); - return distance + cameraOffsetY; - } - return 0; - } - - public double getHorizontalDistanceToTag(double tagHeightMeters) { - if (hasValidTarget()) { - double distance = getStraightDistanceToTag(tagHeightMeters) - cameraOffsetY; - - distance = distance * Math.tan(getTX() * (Math.PI / 180.0)); - return distance + cameraOffsetX; - } - return 0; - } - - @AutoLogOutput - public double getTX() { - return tx * angleMult; - } - - @AutoLogOutput - public double getTY() { - return ty * -angleMult; - } - - public DoubleSupplier tySupplier() { - return () -> getTY(); - } - - public DoubleSupplier txSupplier() { - return () -> getTX(); - } - - // TODO: Do we need these / check if the trig is right - // public double getStraightDistanceToTag() { - // if (hasValidTarget()) - // return goalHeightReef / (Math.tan(Math.toRadians(getTY() + - // limelightOffsetAngleVertical))); - // return 0; - // } - - // TODO: Do we need these / check if the trig is right - - public int getTagID() { - return (int) LimelightHelpers.getFiducialID(cameraName); - } - - public double getDistanceToHub() { - Pose2d robotPose = RobotContainer.drivetrain.getRobotPose(); - Translation2d hubLocation = new Translation2d(hubCenterX, hubCenterY); - Pose2d hubPose = new Pose2d(hubLocation, Rotation2d.fromDegrees(0)); - hubPose = PoseUtils.flipPoseAlliance(hubPose); - return robotPose.getTranslation().getDistance(hubPose.getTranslation()); - } - - public double getAngleToHub() { - Pose2d robotPose = RobotContainer.drivetrain.getRobotPose(); - Translation2d hubLocation = new Translation2d(hubCenterX, hubCenterY); - Pose2d hubPose = new Pose2d(hubLocation, Rotation2d.fromDegrees(0)); - hubPose = PoseUtils.flipPoseAlliance(hubPose); - Translation2d robotToHub = hubPose.getTranslation().minus(robotPose.getTranslation()); - return robotToHub.getAngle().getRotations(); - } - - public void periodic() { - - if (Math.abs(RobotContainer.drivetrain.getPigeon2().getPitch().getValueAsDouble()) > 0.3 - || Math.abs(RobotContainer.drivetrain.getPigeon2().getRoll().getValueAsDouble()) > 0.3) { - hasTipped = true; - } - // tagID = (int) Limetable.getEntry("tid").getDouble(-1); - // TODO if you get a pose estimate in the frame before this is applied it may - // not work - tx = LimelightHelpers.getTX(cameraName); - ty = LimelightHelpers.getTY(cameraName); - RawFiducial[] allTags = LimelightHelpers.getRawFiducials(cameraName); - int numValidTags = 0; - for (LimelightHelpers.RawFiducial t : allTags) { - if (t.distToCamera < 4.0) { - numValidTags++; - } - } - - int[] validTags = new int[numValidTags]; - int counter = 0; - for (RawFiducial t : allTags) { - if (t.distToCamera < 4.0) { - validTags[counter] = t.id; - counter++; - } - } - - double[] poseArr = LimelightHelpers.getBotPose_TargetSpace(cameraName); - Pose2d botPose = new Pose2d(); - if (poseArr.length >= 6) { - botPose = new Pose2d(poseArr[0], poseArr[2], Rotation2d.fromDegrees(poseArr[4])); - } - Logger.recordOutput( - cameraName + "/IMUYaw", - LimelightHelpers.getIMUData(cameraName).robotYaw - * (Math.PI / 180.0)); // TODO should be yaw? - Logger.recordOutput(cameraName + "/BotPoseTargetSpace", botPose); - Logger.recordOutput( - cameraName + "/BotPose3dTargetSpace", - LimelightHelpers.getBotPose3d_TargetSpace(cameraName)); - - var entry = LimelightHelpers.getLimelightNTTableEntry(cameraName, "tcornxy"); - if (entry != null) { - var tcornxy = entry.getDoubleArray(new double[0]); - if (tcornxy != null && tcornxy.length > 0) { - Logger.recordOutput(cameraName + "/tcornxy", tcornxy); - } - } - } - - public Command flashLEDs() { - return Commands.sequence( - Commands.runOnce(() -> LimelightHelpers.setLEDMode_ForceBlink(cameraName)), - Commands.waitSeconds(0.6), - Commands.runOnce(() -> LimelightHelpers.setLEDMode_ForceOff(cameraName))); - } - - public Command ifHasTarget(Command cmd) { - return cmd.onlyWhile(this::hasValidTarget); - } -} diff --git a/src/main/java/frc/robot/subsystems/vision/LimelightHelpers.java b/src/main/java/frc/robot/subsystems/vision/LimelightHelpers.java deleted file mode 100644 index cb6758c0..00000000 --- a/src/main/java/frc/robot/subsystems/vision/LimelightHelpers.java +++ /dev/null @@ -1,1692 +0,0 @@ -// LimelightHelpers v1.11 (REQUIRES LLOS 2025.0 OR LATER) - -package frc.robot.subsystems.vision; - -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 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.Rotation3d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.util.Units; -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.vision.LimelightHelpers.LimelightResults; -import frc.robot.subsystems.vision.LimelightHelpers.PoseEstimate; -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 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; - } - } - - /** 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]; - } - } - } - - private static ObjectMapper mapper; - - /** Print JSON Parse time to the console in milliseconds */ - static boolean profileJSON = false; - - static final String sanitizeName(String name) { - if ("".equals(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); - } - - /** - * 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); - } - - ///// - ///// - - 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(); - } - } - - /** - * Configures the IMU mode for MegaTag2 Localization - * - * @param limelightName Name/identifier of the Limelight - * @param mode IMU mode. - */ - public static void SetIMUMode(String limelightName, int mode) { - setLimelightNTDouble(limelightName, "imumode_set", mode); - } - - /** - * 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 && !"".equals(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/vision2/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java similarity index 97% rename from src/main/java/frc/robot/subsystems/vision2/Vision.java rename to src/main/java/frc/robot/subsystems/vision/Vision.java index 127e2808..ee459d25 100644 --- a/src/main/java/frc/robot/subsystems/vision2/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -1,3 +1,4 @@ +package frc.robot.subsystems.vision; // Copyright (c) 2021-2026 Littleton Robotics // http://github.com/Mechanical-Advantage // @@ -5,9 +6,7 @@ // license that can be found in the LICENSE file // at the root directory of this project. -package frc.robot.subsystems.vision2; - -import static frc.robot.subsystems.vision2.VisionConstants.*; +import static frc.robot.subsystems.vision.VisionConstants.*; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; @@ -19,7 +18,7 @@ import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.subsystems.vision2.VisionIO.PoseObservationType; +import frc.robot.subsystems.vision.VisionIO.PoseObservationType; import java.util.LinkedList; import java.util.List; import org.littletonrobotics.junction.Logger; diff --git a/src/main/java/frc/robot/subsystems/vision2/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java similarity index 96% rename from src/main/java/frc/robot/subsystems/vision2/VisionConstants.java rename to src/main/java/frc/robot/subsystems/vision/VisionConstants.java index d337cb71..f4bbb26f 100644 --- a/src/main/java/frc/robot/subsystems/vision2/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -5,7 +5,7 @@ // license that can be found in the LICENSE file // at the root directory of this project. -package frc.robot.subsystems.vision2; +package frc.robot.subsystems.vision; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; @@ -18,7 +18,7 @@ public class VisionConstants { AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); // Camera names, must match names configured on coprocessor - //TODO: name these better + // TODO: name these better public static String camera0Name = "camera_0"; public static String camera1Name = "camera_1"; diff --git a/src/main/java/frc/robot/subsystems/vision2/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java similarity index 97% rename from src/main/java/frc/robot/subsystems/vision2/VisionIO.java rename to src/main/java/frc/robot/subsystems/vision/VisionIO.java index 9dfb30af..ce7759cf 100644 --- a/src/main/java/frc/robot/subsystems/vision2/VisionIO.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIO.java @@ -5,7 +5,7 @@ // license that can be found in the LICENSE file // at the root directory of this project. -package frc.robot.subsystems.vision2; +package frc.robot.subsystems.vision; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; diff --git a/src/main/java/frc/robot/subsystems/vision2/VisionIOLimelight.java b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java similarity index 99% rename from src/main/java/frc/robot/subsystems/vision2/VisionIOLimelight.java rename to src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java index 7914e5d5..3324e68d 100644 --- a/src/main/java/frc/robot/subsystems/vision2/VisionIOLimelight.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java @@ -5,7 +5,7 @@ // license that can be found in the LICENSE file // at the root directory of this project. -package frc.robot.subsystems.vision2; +package frc.robot.subsystems.vision; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; diff --git a/src/main/java/frc/robot/subsystems/vision2/VisionIOPhotonVision.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java similarity index 97% rename from src/main/java/frc/robot/subsystems/vision2/VisionIOPhotonVision.java rename to src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java index bd2d800a..a5b512b9 100644 --- a/src/main/java/frc/robot/subsystems/vision2/VisionIOPhotonVision.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java @@ -5,9 +5,9 @@ // license that can be found in the LICENSE file // at the root directory of this project. -package frc.robot.subsystems.vision2; +package frc.robot.subsystems.vision; -import static frc.robot.subsystems.vision2.VisionConstants.*; +import static frc.robot.subsystems.vision.VisionConstants.*; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; diff --git a/src/main/java/frc/robot/subsystems/vision2/VisionIOPhotonVisionSim.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java similarity index 93% rename from src/main/java/frc/robot/subsystems/vision2/VisionIOPhotonVisionSim.java rename to src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java index 4d0f61eb..9e8e706e 100644 --- a/src/main/java/frc/robot/subsystems/vision2/VisionIOPhotonVisionSim.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java @@ -5,9 +5,9 @@ // license that can be found in the LICENSE file // at the root directory of this project. -package frc.robot.subsystems.vision2; +package frc.robot.subsystems.vision; -import static frc.robot.subsystems.vision2.VisionConstants.aprilTagLayout; +import static frc.robot.subsystems.vision.VisionConstants.aprilTagLayout; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Transform3d;