move limelight only conversion into its own io

FarhanJ2 · FarhanJ2 · commit de1af7cfd4a8 · 2026-01-15T08:21:52.000-05:00
diff --git a/src/main/java/org/steelhawks/subsystems/vision/VisionConstants.java b/src/main/java/org/steelhawks/subsystems/vision/VisionConstants.java
@@ -11,6 +11,7 @@
 import edu.wpi.first.math.util.Units;
 import org.steelhawks.RobotContainer;
 import org.steelhawks.subsystems.swerve.Swerve;
+import org.steelhawks.subsystems.vision.VisionConstants.Factors.ObjFactors.LimelightFactors;
 import org.steelhawks.subsystems.vision.objdetect.ObjectVisionIO;
 import org.steelhawks.subsystems.vision.objdetect.ObjectVisionIOLimelight;
 import org.steelhawks.subsystems.vision.objdetect.ObjectVisionIOPhoton;
@@ -37,19 +38,46 @@ default Double[] getFactors() {
         class ObjFactors implements Factors {
             private final Double[] factors;
 
+            public enum LimelightFactors {
+                LIMELIGHT_4(82.0, 56.2),
+                LIMELIGHT_3(62.5, 48.9),
+                LIMELIGHT_2(62.5, 48.9)
+                ;
+
+                private final double horizontalFov;
+                private final double verticalFov;
+
+                LimelightFactors(double horizontalFov, double verticalFov) {
+                    this.horizontalFov = horizontalFov;
+                    this.verticalFov = verticalFov;
+                }
+            }
+
             /**
-             * Only used for Limelight, to calculate confidence score.
+             * Only used for Limelight
              *
-             * @param w1 How much the angle matters to confidence
-             * @param w2 How much the shape matters to confidence
-             * @param w3 How much the area matters to confidence
+             * @param horizontalFov The horizontal FOV of the camera.
+             * @param verticalFov The vertical FOV of the camera.
+             * @param resolutionWidth The current resolution width selected in the Limelight config page.
+             * @param resolutionHeight The current resolution height selected in the Limelight config page.
+             * @param confidence The confidence level set in the Limelight config page.
              */
-            public ObjFactors(double w1, double w2, double w3) {
-                factors = new Double[] { w1, w2, w3 };
+            public ObjFactors(double confidence, double horizontalFov, double verticalFov, double resolutionWidth, double resolutionHeight) {
+                factors = new Double[] {
+                    confidence,
+                    horizontalFov,
+                    verticalFov,
+                    resolutionWidth,
+                    resolutionHeight
+                };
+            }
+
+            public ObjFactors(double confidence, LimelightFactors fov, double resolutionWidth, double resolutionHeight) {
+                this(confidence, fov.horizontalFov, fov.verticalFov, resolutionWidth, resolutionHeight);
             }
 
             public ObjFactors() {
-                this(0.0, 0.0, 0.0);
+                this(0.0, 0.0, 0.0, 0.0, 0.0);
             }
 
             @Override
@@ -181,7 +209,7 @@ public enum CameraType {
                     Units.degreesToRadians(15.0)
                 )
             ),
-            new Factors.ObjFactors(0.5, 0.3, 0.2),
+            new Factors.ObjFactors(0.48, LimelightFactors.LIMELIGHT_4, 1280.0, 800.0),
             CameraType.LIMELIGHT
         )
     };
diff --git a/src/main/java/org/steelhawks/subsystems/vision/objdetect/ObjectVision.java b/src/main/java/org/steelhawks/subsystems/vision/objdetect/ObjectVision.java
@@ -1,6 +1,5 @@
 package org.steelhawks.subsystems.vision.objdetect;
 
-import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.*;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d;
@@ -50,23 +49,21 @@ private void addCoralObservationToPose(ObjectVisionIO.ObjectObservation observat
         if (Constants.loggedValue("CoralObservation/WheelOdomEmpty", oldWheelOdomPose.isEmpty())) {
             return;
         }
-
-        // latency compensation via interpolation
         var estimatedPose = RobotContainer.s_Swerve.getPose();
         var wheelOdometryPose = RobotContainer.s_Swerve.getWheelOdomPose();
         Pose2d fieldToRobot =
             estimatedPose.transformBy(new Transform2d(wheelOdometryPose, oldWheelOdomPose.get()));
         Transform3d robotToCamera =
             Objects.requireNonNull(VisionConstants.getObjDetectConfig())[observation.camIndex()].robotToCamera();
 
-        // Get bounding box corners in PIXEL coordinates
-        double[] txCorners = observation.info().tx();
-        double[] tyCorners = observation.info().ty();
+        // in angle coordinates
+        double[] txCorners = observation.info().tx();  // degrees
+        double[] tyCorners = observation.info().ty();  // degrees
 
-        Logger.recordOutput("CoralObservation/TxCorners", txCorners);
-        Logger.recordOutput("CoralObservation/TyCorners", tyCorners);
+        Constants.loggedValue("CoralObservation/TxCorners_degrees", txCorners);
+        Constants.loggedValue("CoralObservation/TyCorners_degrees", tyCorners);
 
-        // Find bounding box center in pixels
+        // bounding box center in deg
         double minX = Math.min(Math.min(txCorners[0], txCorners[1]),
             Math.min(txCorners[2], txCorners[3]));
         double maxX = Math.max(Math.max(txCorners[0], txCorners[1]),
@@ -76,95 +73,47 @@ private void addCoralObservationToPose(ObjectVisionIO.ObjectObservation observat
         double maxY = Math.max(Math.max(tyCorners[0], tyCorners[1]),
             Math.max(tyCorners[2], tyCorners[3]));
 
-        double centerX_pixels = (minX + maxX) / 2.0;
-        double centerY_pixels = (minY + maxY) / 2.0;
-
-        // Limelight resolution: 1280x800
-        double resolutionWidth = 1280.0;
-        double resolutionHeight = 800.0;
-
-        // Convert pixels to normalized coordinates [-1, 1]
-        // Center of image is at (width/2, height/2)
-        double centerX = (centerX_pixels - resolutionWidth / 2.0) / (resolutionWidth / 2.0);
-        double centerY = -(centerY_pixels - resolutionHeight / 2.0) / (resolutionHeight / 2.0); // negative because Y increases downward in pixels
-
-        Logger.recordOutput("CoralObservation/centerX_pixels", centerX_pixels);
-        Logger.recordOutput("CoralObservation/centerY_pixels", centerY_pixels);
-        Logger.recordOutput("CoralObservation/centerX_normalized", centerX);
-        Logger.recordOutput("CoralObservation/centerY_normalized", centerY);
-
-        // Convert normalized coordinates to angles
-        // limelight 4 H:82° V:56.2°
-        double horizontalFOV = 82; // degrees
-        double verticalFOV = 56.2;   // degrees
+        // in degrees
+        double tx = (minX + maxX) / 2.0;
+        double ty = (minY + maxY) / 2.0;
 
-        double tx = centerX * (horizontalFOV / 2.0); // angle in degrees
-        double ty = centerY * (verticalFOV / 2.0);   // angle in degrees
+        Constants.loggedValue("CoralObservation/tx_degrees", tx);
+        Constants.loggedValue("CoralObservation/ty_degrees", ty);
 
+        // trig
         double cameraHeight = robotToCamera.getZ();
-        double cameraPitch = robotToCamera.getRotation().getY(); // in radians
-
-        // Debug logging
-        Logger.recordOutput("CoralObservation/tx_degrees", tx);
-        Logger.recordOutput("CoralObservation/ty_degrees", ty);
-        Logger.recordOutput("CoralObservation/cameraPitch_radians", cameraPitch);
-
-        // Convert ty from degrees to radians
+        double cameraPitch = robotToCamera.getRotation().getY(); // radians
+        Constants.loggedValue("CoralObservation/cameraPitch_radians", cameraPitch);
         double tyRadians = Math.toRadians(ty);
-
-        // Calculate vertical angle from horizontal
-        // cameraPitch is camera tilt (positive = up), ty is offset from camera center
         double verticalAngleFromHorizontal = cameraPitch + tyRadians;
-
-        Logger.recordOutput("CoralObservation/AngleFromHorizontal_radians", verticalAngleFromHorizontal);
-        Logger.recordOutput("CoralObservation/AngleFromHorizontal_degrees", Math.toDegrees(verticalAngleFromHorizontal));
-
-        // Target must be below horizontal to be valid
+        Constants.loggedValue("CoralObservation/AngleFromHorizontal_radians", verticalAngleFromHorizontal);
+        Constants.loggedValue("CoralObservation/AngleFromHorizontal_degrees", Math.toDegrees(verticalAngleFromHorizontal));
         if (Constants.loggedValue("CoralObservation/VerticalAngleError", verticalAngleFromHorizontal <= 0)) {
             return;
         }
-
-        double targetHeight = 0.0; // coral is on the ground
+        double targetHeight = 0.0;
         double forwardDistance = (cameraHeight - targetHeight) / Math.tan(verticalAngleFromHorizontal);
-
-        // Calculate the translation from camera to coral in CAMERA reference frame
         double txRadians = Math.toRadians(tx);
-
-        // Create vector from camera to coral in camera's frame
-        // X-axis is forward, Y-axis is left
-        Translation2d cameraToCoralInCameraFrame = new Translation2d(
-            forwardDistance * Math.cos(txRadians),  // forward component
-            forwardDistance * Math.sin(txRadians)   // lateral component
-        );
-
-        Logger.recordOutput("CoralObservation/cameraToCoralInCameraFrame",
+        Translation2d cameraToCoralInCameraFrame =
+            new Translation2d(
+                forwardDistance * Math.cos(txRadians),
+                forwardDistance * Math.sin(txRadians));
+        Constants.loggedValue("CoralObservation/cameraToCoralInCameraFrame",
             new Pose2d(cameraToCoralInCameraFrame, new Rotation2d()));
 
-        // Transform camera pose to field coordinates
         Transform2d robotToCamera2d = new Transform2d(
             robotToCamera.getTranslation().toTranslation2d(),
             robotToCamera.getRotation().toRotation2d());
         Pose2d fieldToCamera = fieldToRobot.transformBy(robotToCamera2d);
 
-        // Debug logging
-        Logger.recordOutput("CoralObservation/robotToCamera2d", robotToCamera2d);
-        Logger.recordOutput("CoralObservation/cameraYaw_degrees",
-            Math.toDegrees(robotToCamera.getRotation().getZ()));
-        Logger.recordOutput("CoralObservation/fieldToRobot", fieldToRobot);
-        Logger.recordOutput("CoralObservation/fieldToCamera", fieldToCamera);
-        Logger.recordOutput("CoralObservation/forwardDistance", forwardDistance);
-        Logger.recordOutput("CoralObservation/txRadians", txRadians);
-
-        // Transform the camera-frame vector to field frame
-        Pose2d fieldToCoral = fieldToCamera.transformBy(
-            new Transform2d(cameraToCoralInCameraFrame, new Rotation2d())
-        );
-
-        Logger.recordOutput("CoralObservation/fieldToCoral", fieldToCoral);
+        Constants.loggedValue("CoralObservation/fieldToRobot", fieldToRobot);
+        Constants.loggedValue("CoralObservation/fieldToCamera", fieldToCamera);
+        Constants.loggedValue("CoralObservation/forwardDistance", forwardDistance);
 
+        Pose2d fieldToCoral = fieldToCamera
+            .transformBy(new Transform2d(cameraToCoralInCameraFrame, new Rotation2d()));
+        Constants.loggedValue("CoralObservation/fieldToCoral", fieldToCoral);
         CoralPose coralPose = new CoralPose(fieldToCoral.getTranslation(), observation.timestamp());
-
-        // Remove overlapping corals and old detections
         coralPoses.removeIf(
             c -> c.translation.getDistance(fieldToCoral.getTranslation()) <= coralOverlap
                 || now - c.timestamp > coralMaxAge);
@@ -183,8 +132,8 @@ public void periodic() {
             .sorted(Comparator.comparingDouble(ObjectVisionIO.ObjectObservation::timestamp))
             .forEach(this::addCoralObservationToPose);
 
-        coralPoses.stream()
-            .forEach(o -> Logger.recordOutput("CoralDetections/Detection", new Pose2d(o.translation, new Rotation2d())));
+        coralPoses.forEach(o ->
+            Logger.recordOutput("CoralDetections/Detection", new Pose2d(o.translation, new Rotation2d())));
         coralObjects.setPoses(
             coralPoses.stream()
                 .map(coral -> new Pose2d(coral.translation, new Rotation2d())) // zero rotation
diff --git a/src/main/java/org/steelhawks/subsystems/vision/objdetect/ObjectVisionIO.java b/src/main/java/org/steelhawks/subsystems/vision/objdetect/ObjectVisionIO.java
@@ -9,7 +9,7 @@ public interface ObjectVisionIO {
     record ObjectObservation(
         int camIndex,
         DetectionInfo info,
-        double confidence, // keeping because photonvision gives us its own confidence, so no need to calculate it
+        double confidence,
         double timestamp
     ) {}
 
@@ -19,6 +19,14 @@ record DetectionInfo(
         double[] tx,      // Corner X positions
         double[] ty       // Corner Y positions
     ) {
+        public DetectionInfo(LimelightHelpers.RawDetection raw, double[] txs, double[] tys) {
+            this(
+                raw.classId,
+                raw.ta,
+                txs,
+                tys
+            );
+        }
         public DetectionInfo(LimelightHelpers.RawDetection raw) {
             this(
                 raw.classId,
diff --git a/src/main/java/org/steelhawks/subsystems/vision/objdetect/ObjectVisionIOLimelight.java b/src/main/java/org/steelhawks/subsystems/vision/objdetect/ObjectVisionIOLimelight.java
@@ -1,38 +1,57 @@
 package org.steelhawks.subsystems.vision.objdetect;
 
 import edu.wpi.first.networktables.DoubleSubscriber;
-import edu.wpi.first.networktables.IntegerSubscriber;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj.RobotController;
-import org.littletonrobotics.junction.Logger;
+import org.steelhawks.subsystems.vision.VisionConstants;
+import org.steelhawks.subsystems.vision.VisionConstants.Factors.ObjFactors;
 import org.steelhawks.util.LimelightHelpers;
 
 import java.util.LinkedList;
 import java.util.List;
+import java.util.Objects;
 
 public class ObjectVisionIOLimelight implements ObjectVisionIO {
 
     private final String name;
     private final int camIndex;
     private final DoubleSubscriber latencySubscriber;
-    private final DoubleSubscriber txSubscriber;
-    private final DoubleSubscriber tySubscriber;
-    private final DoubleSubscriber taSubscriber; // target area
-    private final IntegerSubscriber targetValidSubscriber;
+
+    private final ObjFactors factors;
 
     public ObjectVisionIOLimelight(String name, int camIndex) {
         this.name = name;
         this.camIndex = camIndex;
         var table = NetworkTableInstance.getDefault().getTable(name);
         latencySubscriber = table.getDoubleTopic("tl").subscribe(0.0);
-        txSubscriber = table.getDoubleTopic("tx").subscribe(0.0);
-        tySubscriber = table.getDoubleTopic("ty").subscribe(0.0);
-        taSubscriber = table.getDoubleTopic("ta").subscribe(0.0);
-        targetValidSubscriber = table.getIntegerTopic("tv").subscribe(0);
+        factors = (ObjFactors) Objects.requireNonNull(VisionConstants.getObjDetectConfig())[camIndex].factors();
+    }
+
+    private double[] convertPixelsToAngles(double[] pixelCorners, boolean isY) {
+        final double resolutionWidth = factors.getFactors()[3];
+        final double resolutionHeight = factors.getFactors()[4];
+        final double horizontalFov = factors.getFactors()[1];
+        final double verticalFov = factors.getFactors()[2];
+
+        double resolution = isY ? resolutionHeight : resolutionWidth;
+        double fov = isY ? verticalFov : horizontalFov;
+
+        double[] angles = new double[pixelCorners.length];
+        for (int i = 0; i < pixelCorners.length; i++) {
+            // norm to [-1, 1]
+            double normalized = (pixelCorners[i] - resolution / 2.0) / (resolution / 2.0);
+            if (isY) normalized = -normalized; // y increases downward
+
+            // convert to angle
+            angles[i] = normalized * (fov / 2.0);
+        }
+        return angles;
     }
 
     @Override
     public void updateInputs(ObjectVisionIOInputs inputs) {
+        final double confidence = factors.getFactors()[0];
+
         inputs.connected =
             ((RobotController.getFPGATime() - latencySubscriber.getLastChange()) / 1000) < 250;
 
@@ -43,11 +62,19 @@ public void updateInputs(ObjectVisionIOInputs inputs) {
         for (LimelightHelpers.RawDetection detection : detections) {
             double limelightLatencySec = latencySubscriber.get() / 1000.0;
             double timestamp = RobotController.getFPGATime() / 1e6 - limelightLatencySec;
+            double[] convertedCornersX = convertPixelsToAngles(
+                new double[] {
+                    detection.corner0_X, detection.corner1_X,
+                    detection.corner2_X, detection.corner3_X}, false);
+            double[] convertedCornersY = convertPixelsToAngles(
+                new double[] {
+                    detection.corner0_Y, detection.corner1_Y,
+                    detection.corner2_Y, detection.corner3_Y}, true);
             observations.add(
                 new ObjectObservation(
                     camIndex,
-                    new DetectionInfo(detection),
-                    0.48, // 0.0 because limelight doesn't give you a confidence score, we calculate it ourselves
+                    new DetectionInfo(detection, convertedCornersX, convertedCornersY),
+                    confidence,
                     timestamp // timestamp in seconds
                 )
             );
