Vertical aiming

src/main/java/frc/robot/subsystems/HubTargetData.java

@@ -23,9 +23,11 @@
 public class HubTargetData extends TargetListTargetData {
 
   static int kFrameCenter = Integer.MAX_VALUE;
+  static int kFrameVerticalCenter = Integer.MAX_VALUE;
   private final double errorPixels;
   private final double range;
   private final Logger logger = LoggerFactory.getLogger(this.getClass());
+  public boolean evenNumTargets = false;
 
   public HubTargetData() {
     super();
@@ -77,6 +79,17 @@ public double getErrorPixelsJetson() {
     return errorPixels;
   }
 
+  public double getVerticalOffsetPixels() {
+    int maxY = targets.get(Math.round(targets.size() / 2)).topLeft.y;
+    return maxY;
+  }
+
+  public double getVerticalOffsetRadians() {
+    return VisionConstants.kHorizonFov
+        * getVerticalOffsetPixels()
+        / (kFrameVerticalCenter * 2); // FIXME kVerticalFov?
+  }
+
   /**
    * Return the angle from the center of the Hub target group to the center of the camera frame. You
    * should check {@link #isValid()} before calling this method.
@@ -85,7 +98,9 @@ public double getErrorPixelsJetson() {
    * @throws IndexOutOfBoundsException if the list of targets is empty
    */
   public double getErrorRadians() {
-    return -VisionConstants.kVerticalFov * getErrorPixels() / (kFrameCenter * 2);
+    return -VisionConstants.kVerticalFov
+        * getErrorPixels()
+        / (kFrameCenter * 2); // FIXME kHorizonFov?
   }
 
   /**
@@ -111,16 +126,45 @@ public int testGetTargetsPixelWidth() {
       Rect rightTarget = targets.get((targets.size() - 1) / 2 + 1);
 
       pixelWidth = rightTarget.bottomRight.x - leftTarget.topLeft.x;
+      evenNumTargets = false;
     } else {
       Rect leftTarget = targets.get(targets.size() / 2 - 2);
       Rect rightTarget = targets.get(targets.size() / 2 + 1);
 
       pixelWidth = rightTarget.topLeft.x - leftTarget.bottomRight.x;
+      evenNumTargets = true;
     }
 
     return pixelWidth;
   }
 
+  public boolean isNumTargetsEven() {
+    return evenNumTargets;
+  }
+
+  public int getVerticalPixelHeight() {
+    int pixelHeight;
+    if (targets.size() % 2 == 1) {
+      Rect centerTarget = targets.get((targets.size() - 1) / 2);
+      Rect rightTarget = targets.get((targets.size() - 1) / 2 + 1);
+      Rect leftTarget = targets.get((targets.size() - 1) / 2 - 1);
+
+      pixelHeight =
+          (rightTarget.bottomRight.y + leftTarget.bottomRight.y) / 2 - centerTarget.topLeft.y;
+    } else {
+      Rect rightCenterTarget = targets.get(targets.size() / 2);
+      Rect rightTarget = targets.get(targets.size() / 2 + 1);
+      Rect leftCenterTarget = targets.get(targets.size() / 2 - 1);
+      Rect leftTarget = targets.get(targets.size() / 2 - 2);
+
+      pixelHeight =
+          ((rightTarget.bottomRight.y - rightCenterTarget.topLeft.y)
+                  + (leftTarget.bottomRight.y - leftCenterTarget.topLeft.y))
+              / 2;
+    }
+    return pixelHeight;
+  }
+
   public double testGetDistance() {
     double pixelWidth;
 

src/main/java/frc/robot/subsystems/VisionSubsystem.java

@@ -38,6 +38,7 @@ public VisionSubsystem(DriveSubsystem driveSubsystem) {
     deadeye = new Deadeye<>("A0", HubTargetData.class, networkTableInstance);
     deadeye.setTargetDataListener(this);
     HubTargetData.kFrameCenter = deadeye.getCapture().width / 2;
+    HubTargetData.kFrameVerticalCenter = deadeye.getCapture().height / 2;
   }
 
   @Override
@@ -74,12 +75,26 @@ public void disable() {
         new Measure("Target Data Valid", this::getValid),
         new Measure("Test Pixel Width", this::getTargetsDistancePixel),
         new Measure("Test Ground Distance", this::getTargetsDistanceGround),
+        new Measure("Is Even Num Of Targets", () -> isNumTargetsEven() ? 1.0 : 0.0),
+        new Measure("Vertical Pixel Height", this::getVerticalPixelHeight),
+        new Measure("Vertical Radian Offset", this::getVerticalOffsetRadians),
+        new Measure("Target Data SN", () -> targetData.serial),
         new Measure("Target Data SN", () -> targetData.serial),
         new Measure("Ranging Valid", this::getRangingValid));
   }
 
   // these private getters are for the grapher and prevent a data race that can occur if targetData
   // updates after isValid() and before getErrorXXX()
+  private boolean isNumTargetsEven() {
+    var td = targetData;
+    return td.isValid() ? td.isNumTargetsEven() : false;
+  }
+
+  private double getVerticalPixelHeight() {
+    var td = targetData;
+    return td.isValid() ? td.getVerticalPixelHeight() : 2767.0;
+  }
+
   private double getErrorPixels() {
     var td = targetData;
     return td.isValid() ? td.getErrorPixels() : 2767.0;
@@ -100,6 +115,11 @@ private double getErrorDegrees() {
     return td.isValid() ? Math.toDegrees(td.getErrorRadians()) : 2767.0;
   }
 
+  private double getVerticalOffsetRadians() {
+    var td = targetData;
+    return td.isValid() ? targetData.getVerticalOffsetRadians() : 2767.0;
+  }
+
   // not used
   public double getTargetsDistancePixel() {
     var td = targetData;