diff --git a/src/main/java/frc/robot/subsystems/HubTargetData.java b/src/main/java/frc/robot/subsystems/HubTargetData.java index 0d1d266..bdf9425 100644 --- a/src/main/java/frc/robot/subsystems/HubTargetData.java +++ b/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; diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java index 02ed583..5baf165 100644 --- a/src/main/java/frc/robot/subsystems/VisionSubsystem.java +++ b/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;