From 193623f71316983ee951249b2177578e2c7d07b7 Mon Sep 17 00:00:00 2001 From: JE514 Date: Thu, 2 Jun 2022 20:22:23 -0400 Subject: [PATCH 1/4] Vertical Aiming Integration --- .../frc/robot/subsystems/HubTargetData.java | 30 +++++++++++++++++++ .../frc/robot/subsystems/VisionSubsystem.java | 10 +++++++ 2 files changed, 40 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/HubTargetData.java b/src/main/java/frc/robot/subsystems/HubTargetData.java index b6d1137..3171880 100644 --- a/src/main/java/frc/robot/subsystems/HubTargetData.java +++ b/src/main/java/frc/robot/subsystems/HubTargetData.java @@ -26,6 +26,7 @@ public class HubTargetData extends TargetListTargetData { private final double errorPixels; private final double range; private final Logger logger = LoggerFactory.getLogger(this.getClass()); + public boolean evenNumTargets = false; public HubTargetData() { super(); @@ -107,16 +108,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 409aa18..73b330a 100644 --- a/src/main/java/frc/robot/subsystems/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSubsystem.java @@ -74,11 +74,21 @@ 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("Target Data SN", () -> targetData.serial)); } // 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() { + return targetData.isNumTargetsEven(); + } + + private double getVerticalPixelHeight() { + return targetData.getVerticalPixelHeight(); + } + private double getErrorPixels() { var td = targetData; return td.isValid() ? td.getErrorPixels() : 2767.0; From ad3cc36a3ed468fae7a02a03df3c795e89b3841b Mon Sep 17 00:00:00 2001 From: JE514 Date: Tue, 7 Jun 2022 20:18:17 -0400 Subject: [PATCH 2/4] not tested --- .../java/frc/robot/subsystems/HubTargetData.java | 12 +++++++++++- .../java/frc/robot/subsystems/VisionSubsystem.java | 5 +++++ 2 files changed, 16 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/HubTargetData.java b/src/main/java/frc/robot/subsystems/HubTargetData.java index 3171880..2c1b5b1 100644 --- a/src/main/java/frc/robot/subsystems/HubTargetData.java +++ b/src/main/java/frc/robot/subsystems/HubTargetData.java @@ -5,6 +5,7 @@ import com.squareup.moshi.JsonReader; import com.squareup.moshi.JsonWriter; import edu.wpi.first.math.geometry.Rotation2d; +import frc.robot.Constants; import frc.robot.Constants.VisionConstants; import java.io.IOException; import java.util.ArrayList; @@ -23,6 +24,7 @@ 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()); @@ -74,6 +76,14 @@ 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. @@ -82,7 +92,7 @@ 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? } /** diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java index 73b330a..025a3d3 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 @@ -76,6 +77,7 @@ public void disable() { 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)); } @@ -108,6 +110,9 @@ private double getErrorDegrees() { var td = targetData; return td.isValid() ? Math.toDegrees(td.getErrorRadians()) : 2767.0; } + private double getVerticalOffsetRadians() { + return targetData.getVerticalOffsetRadians(); + } // not used public double getTargetsDistancePixel() { From 7937546d483aecd78fd2c3ec578d37ad251d0e82 Mon Sep 17 00:00:00 2001 From: JE514 Date: Tue, 7 Jun 2022 20:25:10 -0400 Subject: [PATCH 3/4] Fixed crash grapher --- src/main/java/frc/robot/subsystems/VisionSubsystem.java | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java index cedc5bf..6920e91 100644 --- a/src/main/java/frc/robot/subsystems/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSubsystem.java @@ -86,11 +86,13 @@ public void disable() { // 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() { - return targetData.isNumTargetsEven(); + var td = targetData; + return td.isValid() ? td.isNumTargetsEven() : false; } private double getVerticalPixelHeight() { - return targetData.getVerticalPixelHeight(); + var td = targetData; + return td.isValid() ? td.getVerticalPixelHeight() : 2767.0; } private double getErrorPixels() { @@ -113,7 +115,8 @@ private double getErrorDegrees() { return td.isValid() ? Math.toDegrees(td.getErrorRadians()) : 2767.0; } private double getVerticalOffsetRadians() { - return targetData.getVerticalOffsetRadians(); + var td = targetData; + return td.isValid() ? targetData.getVerticalOffsetRadians() : 2767.0; } // not used From 3ea0b5d6159a91fc9b34ae061e77b2230e038ee9 Mon Sep 17 00:00:00 2001 From: IRoot <74614800+imroot07@users.noreply.github.com> Date: Thu, 9 Jun 2022 20:23:01 -0400 Subject: [PATCH 4/4] spotlessApply and build sheck --- .../java/frc/robot/subsystems/HubTargetData.java | 14 +++++++++----- .../java/frc/robot/subsystems/VisionSubsystem.java | 3 ++- 2 files changed, 11 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/HubTargetData.java b/src/main/java/frc/robot/subsystems/HubTargetData.java index 159e9c5..bdf9425 100644 --- a/src/main/java/frc/robot/subsystems/HubTargetData.java +++ b/src/main/java/frc/robot/subsystems/HubTargetData.java @@ -5,7 +5,6 @@ import com.squareup.moshi.JsonReader; import com.squareup.moshi.JsonWriter; import edu.wpi.first.math.geometry.Rotation2d; -import frc.robot.Constants; import frc.robot.Constants.VisionConstants; import java.io.IOException; import java.util.ArrayList; @@ -81,11 +80,14 @@ public double getErrorPixelsJetson() { } public double getVerticalOffsetPixels() { - int maxY = targets.get(Math.round(targets.size() / 2)).topLeft.y; - return maxY; + 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 VisionConstants.kHorizonFov + * getVerticalOffsetPixels() + / (kFrameVerticalCenter * 2); // FIXME kVerticalFov? } /** @@ -96,7 +98,9 @@ public double getVerticalOffsetRadians() { * @throws IndexOutOfBoundsException if the list of targets is empty */ public double getErrorRadians() { - return -VisionConstants.kVerticalFov * getErrorPixels() / (kFrameCenter * 2); //FIXME kHorizonFov? + return -VisionConstants.kVerticalFov + * getErrorPixels() + / (kFrameCenter * 2); // FIXME kHorizonFov? } /** diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java index 6920e91..5baf165 100644 --- a/src/main/java/frc/robot/subsystems/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSubsystem.java @@ -78,7 +78,7 @@ public void disable() { 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("Target Data SN", () -> targetData.serial), new Measure("Ranging Valid", this::getRangingValid)); } @@ -114,6 +114,7 @@ private double getErrorDegrees() { var td = targetData; return td.isValid() ? Math.toDegrees(td.getErrorRadians()) : 2767.0; } + private double getVerticalOffsetRadians() { var td = targetData; return td.isValid() ? targetData.getVerticalOffsetRadians() : 2767.0;