From 12c744b403ee779f6a39aadedf2eaddaaa00c2e3 Mon Sep 17 00:00:00 2001 From: Hannahjjj97 Date: Wed, 12 Feb 2025 19:03:11 +0800 Subject: [PATCH 01/23] =?UTF-8?q?feat:=20CameraServer=20=E5=8A=A0=E5=85=A5?= =?UTF-8?q?AutomaticCapture()?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/java/frc/robot/Robot.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 67056c02..34681474 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,6 +4,7 @@ package frc.robot; +import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.TimedRobot; @@ -19,6 +20,8 @@ public class Robot extends TimedRobot { public Robot() { m_robotContainer = new RobotContainer(); + CameraServer.startAutomaticCapture(); + } @Override From 503dafd6b2e91f7bcb8cc543752caa2705bdac89 Mon Sep 17 00:00:00 2001 From: Hannahjjj97 Date: Wed, 12 Feb 2025 19:22:14 +0800 Subject: [PATCH 02/23] =?UTF-8?q?feat:=20TagTracking=20Constants=20?= =?UTF-8?q?=E6=8A=8Alimelight=E7=9A=84=E7=A8=8B=E5=BC=8F=E4=B8=9F=E9=80=B2?= =?UTF-8?q?=E4=BE=86?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/java/frc/robot/Constants.java | 7 + .../java/frc/robot/vision/TagTracking.java | 238 ++++++++++++++++++ 2 files changed, 245 insertions(+) create mode 100644 src/main/java/frc/robot/vision/TagTracking.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 599add33..0a21656d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -27,4 +27,11 @@ public static final class PowerDistributionConstant { public static final double kRampMotorMaxCurrent = 40; } + public static final class TagTrackingConstant { + public static final double kRampHeight = 0.0; + public static final double kCamHeight = 0.615; + public static final double kCamPitch = 10.0; + public static final double kCamToRampDistance = 0.11; + } + } diff --git a/src/main/java/frc/robot/vision/TagTracking.java b/src/main/java/frc/robot/vision/TagTracking.java new file mode 100644 index 00000000..6f55936b --- /dev/null +++ b/src/main/java/frc/robot/vision/TagTracking.java @@ -0,0 +1,238 @@ +package frc.robot.vision; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.Constants.TagTrackingConstant; +import java.io.IOException; +import java.util.Optional; + +public class TagTracking { + private final NetworkTable table; + private final AprilTagFieldLayout layout; + + private double tv; + private double tx; + private double ty; + private double id; + + private double[] bt; // botpose_targetspace + private double[] ct; // camerapose_targetspace + + private double distance; + private boolean isCamOn = true; + + public TagTracking() { + table = NetworkTableInstance.getDefault().getTable("limelight"); + setCamMode(); + setLedMode(0); + setPipeline(0); + try { + layout = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2024Crescendo.m_resourceFile); + } catch (IOException err) { + throw new RuntimeException(); + } + } + + /** + * Set desired limelight operation mode. 0 is vision processor. 1 is Driver + * Camera (Increases exposure, disables vision processing) + * + * @param CamMode set 0 plz + * + */ + public void setCamMode() { + if (isCamOn) { + table.getEntry("camMode").setNumber(0); + } else { + table.getEntry("camMode").setNumber(1); + } + } + + /** + * Set desired green light state. 0 is default. 1 is force off. 2 is force + * blink. 3 is force on. + * + * @param ledMode set 0 or 1 plz + * + */ + private void setLedMode(int ledMode) { + table.getEntry("ledMode").setNumber(ledMode); + } + + /** + * Set desired limelight pipeline. + * + * @param pipeline in this game let's set 0 + * + */ + private void setPipeline(int pipeline) { + table.getEntry("pipeline").setNumber(pipeline); + } + + /** + * Returns the x offset between the tag and crosshair. + * + * @return x offset + * + */ + public double getTx() { + tx = table.getEntry("tx").getDouble(0); + return tx; + } + + /** + * Returns the y offset between the tag and crosshair. + * + * @return y offset + * + */ + public double getTy() { + ty = table.getEntry("ty").getDouble(0); + return ty; + } + + /** + * Returns 1 if a tag is detected. 0 if none. + * + * @return 0 or 1 + * + */ + public double getTv() { + tv = table.getEntry("tv").getDouble(0); + return tv; + } + + /** + * Returns the fiducial tag's ID (double); + * + * @return tag ID + * + */ + public double getTID() { + id = table.getEntry("tid").getDouble(0); + return id; + } + + /** + * Returns a double array of botpose in target space. The former 3 refers to + * translation, while the latter 3 refers to rotation (in the sequence of roll, + * pitch, yaw) In target space, (0,0,0) is the centre of the tag, x+ points to + * the right side (when you're facing the tag), y+ points down, z+ points to + * front. + * + * @return x, y, z, roll, pitch, yaw + */ + public double[] getBT() { + bt = table.getEntry("botpose_targetspace").getDoubleArray(new double[6]); + return bt; + } + + /** + * Returns a double array of campose in target space. The former 3 refers to + * translation, while the latter 3 refers to rotation (in the sequence of roll, + * pitch, yaw) In target space, (0,0,0) is the centre of the tag, x+ points to + * the right side (when you're facing the tag), y+ points down, z+ points to + * front. + * + * @return x, y, z, roll, pitch, yaw + */ + public double[] getCT() { + ct = table.getEntry("camerapose_targetspace").getDoubleArray(new double[6]); + return ct; + } + + /** + * Returns bot to tag's direct distance. + * + * @return distance (double) + */ + public double getDistance() { + // readValue(); + double targetHeight = getBT()[1]; // botpose in targetspace y + double xDis = getBT()[0]; + double zDis = getBT()[2]; + double horDis = Math.sqrt(Math.pow(xDis, 2) + Math.pow(zDis, 2)); + distance = Math.sqrt(Math.pow(targetHeight, 2) + Math.pow(horDis, 2)); + return distance; + } + + public double getHorizontalDistanceByCT() { + double horDis = Math.sqrt( + (Math.pow(getCT()[2] + TagTrackingConstant.kCamToRampDistance, 2.0) + Math.pow(getCT()[0], 2.0))); + SmartDashboard.putNumber("Robottotagdistance", horDis); + return horDis; + } + + /** + * Not yet experimented. Return shooter to goal angle degree by calculating with + * tx and ty. + * + * @return shooter to goal angle (degree) + */ + public double getHorDistanceByCal() { + double pitch = getTy(); + double yaw = getTx(); + double y = TagTrackingConstant.kCamHeight + * (1 / Math.tan(Math.toRadians(pitch + TagTrackingConstant.kCamPitch))); + double x = y * Math.tan(Math.toRadians(yaw)); + double horDistance = Math.sqrt(Math.pow(y, 2.0) + Math.pow(x, 2.0)); + return horDistance; + } + + /** + * Gets the tag's pose in 2 dimension + * + * @return tagPose + */ + public Pose2d getTagPose2d() { + if (getTv() == 1) { + Optional tag_Pose3d = layout.getTagPose((int) getTID()); + Pose2d tagPose2d = tag_Pose3d.isPresent() ? tag_Pose3d.get().toPose2d() : new Pose2d(); + return tagPose2d; + } else { + return new Pose2d(); + } + } + + /** + * Gets the tag's pose in 3 dimension + * + * @return tagPose + */ + public Pose3d getTagPose3d() { + if (getTv() == 1) { + Optional tag_Pose3d = layout.getTagPose((int) getTID()); + Pose3d tagPose = tag_Pose3d.isPresent() ? tag_Pose3d.get() : new Pose3d(); + return tagPose; + } else { + return new Pose3d(); + } + } + + public Pose2d getDesiredTagPose2d(double index) { + if (getTv() == 1) { + Optional tag_Pose3d = layout.getTagPose((int) index); + Pose2d tagPose2d = tag_Pose3d.isPresent() ? tag_Pose3d.get().toPose2d() : new Pose2d(); + return tagPose2d; + } else { + return new Pose2d(); + } + } + + public void isVisionOn() { + isCamOn = !isCamOn; + } + + public void setAutoCamOn() { + table.getEntry("camMode").setNumber(0); + } + + public void setAutoCamOff() { + table.getEntry("camMode").setNumber(1); + } +} From f1e8873986cec64d1832af6e92b8c787f3e81d99 Mon Sep 17 00:00:00 2001 From: capy0827 Date: Wed, 12 Feb 2025 20:50:22 +0800 Subject: [PATCH 03/23] style: styleCheck some modifications to pass styleCheck --- src/main/java/frc/robot/vision/TagTracking.java | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/vision/TagTracking.java b/src/main/java/frc/robot/vision/TagTracking.java index 6f55936b..285665b6 100644 --- a/src/main/java/frc/robot/vision/TagTracking.java +++ b/src/main/java/frc/robot/vision/TagTracking.java @@ -33,7 +33,7 @@ public TagTracking() { setPipeline(0); try { layout = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2024Crescendo.m_resourceFile); - } catch (IOException err) { + } catch (IOException err) {s throw new RuntimeException(); } } @@ -41,8 +41,7 @@ public TagTracking() { /** * Set desired limelight operation mode. 0 is vision processor. 1 is Driver * Camera (Increases exposure, disables vision processing) - * - * @param CamMode set 0 plz + * @param setCamMode set 0 plz * */ public void setCamMode() { @@ -56,7 +55,6 @@ public void setCamMode() { /** * Set desired green light state. 0 is default. 1 is force off. 2 is force * blink. 3 is force on. - * * @param ledMode set 0 or 1 plz * */ @@ -66,7 +64,6 @@ private void setLedMode(int ledMode) { /** * Set desired limelight pipeline. - * * @param pipeline in this game let's set 0 * */ @@ -76,7 +73,6 @@ private void setPipeline(int pipeline) { /** * Returns the x offset between the tag and crosshair. - * * @return x offset * */ @@ -87,7 +83,6 @@ public double getTx() { /** * Returns the y offset between the tag and crosshair. - * * @return y offset * */ From 83e92b835327cb2314082aa7cff15668ed935578 Mon Sep 17 00:00:00 2001 From: capy0827 Date: Wed, 12 Feb 2025 20:53:29 +0800 Subject: [PATCH 04/23] =?UTF-8?q?fix:=20=E6=89=8B=E6=AE=98=E6=8C=89?= =?UTF-8?q?=E5=88=B0=E7=9A=84=20=E6=8A=8A=E6=89=8B=E6=AE=98=E6=8C=89?= =?UTF-8?q?=E5=88=B0=E7=9A=84=E5=88=AA=E6=8E=89?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/java/frc/robot/vision/TagTracking.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/vision/TagTracking.java b/src/main/java/frc/robot/vision/TagTracking.java index 285665b6..86b5be1e 100644 --- a/src/main/java/frc/robot/vision/TagTracking.java +++ b/src/main/java/frc/robot/vision/TagTracking.java @@ -33,7 +33,7 @@ public TagTracking() { setPipeline(0); try { layout = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2024Crescendo.m_resourceFile); - } catch (IOException err) {s + } catch (IOException err) { throw new RuntimeException(); } } From 6d2f9f54af68ff825a0da6492cd3e4bdb0227237 Mon Sep 17 00:00:00 2001 From: capy0827 Date: Wed, 12 Feb 2025 20:56:05 +0800 Subject: [PATCH 05/23] style: styleCheck some modifications to pass styleCheck --- .../java/frc/robot/vision/TagTracking.java | 98 +++---------------- 1 file changed, 15 insertions(+), 83 deletions(-) diff --git a/src/main/java/frc/robot/vision/TagTracking.java b/src/main/java/frc/robot/vision/TagTracking.java index 86b5be1e..39dd9973 100644 --- a/src/main/java/frc/robot/vision/TagTracking.java +++ b/src/main/java/frc/robot/vision/TagTracking.java @@ -38,114 +38,59 @@ public TagTracking() { } } - /** - * Set desired limelight operation mode. 0 is vision processor. 1 is Driver - * Camera (Increases exposure, disables vision processing) - * @param setCamMode set 0 plz - * - */ public void setCamMode() { if (isCamOn) { table.getEntry("camMode").setNumber(0); - } else { + + * else { table.getEntry("camMode").setNumber(1); } } - /** - * Set desired green light state. 0 is default. 1 is force off. 2 is force - * blink. 3 is force on. - * @param ledMode set 0 or 1 plz - * - */ private void setLedMode(int ledMode) { table.getEntry("ledMode").setNumber(ledMode); } - /** - * Set desired limelight pipeline. - * @param pipeline in this game let's set 0 - * - */ + private void setPipeline(int pipeline) { table.getEntry("pipeline").setNumber(pipeline); } - /** - * Returns the x offset between the tag and crosshair. - * @return x offset + * - */ public double getTx() { tx = table.getEntry("tx").getDouble(0); return tx; } - /** - * Returns the y offset between the tag and crosshair. - * @return y offset - * - */ + public double getTy() { ty = table.getEntry("ty").getDouble(0); - return ty; + + * eturn ty; } - /** - * Returns 1 if a tag is detected. 0 if none. - * - * @return 0 or 1 - * - */ + public double getTv() { tv = table.getEntry("tv").getDouble(0); return tv; } - /** - * Returns the fiducial tag's ID (double); + * - * @return tag ID - * - */ public double getTID() { id = table.getEntry("tid").getDouble(0); return id; } - /** - * Returns a double array of botpose in target space. The former 3 refers to - * translation, while the latter 3 refers to rotation (in the sequence of roll, - * pitch, yaw) In target space, (0,0,0) is the centre of the tag, x+ points to - * the right side (when you're facing the tag), y+ points down, z+ points to - * front. - * - * @return x, y, z, roll, pitch, yaw - */ + public double[] getBT() { bt = table.getEntry("botpose_targetspace").getDoubleArray(new double[6]); return bt; - } - - /** - * Returns a double array of campose in target space. The former 3 refers to - * translation, while the latter 3 refers to rotation (in the sequence of roll, - * pitch, yaw) In target space, (0,0,0) is the centre of the tag, x+ points to - * the right side (when you're facing the tag), y+ points down, z+ points to - * front. + } * - * @return x, y, z, roll, pitch, yaw - */ - public double[] getCT() { - ct = table.getEntry("camerapose_targetspace").getDoubleArray(new double[6]); - return ct; - } - /** - * Returns bot to tag's direct distance. - * - * @return distance (double) - */ + public double getDistance() { // readValue(); double targetHeight = getBT()[1]; // botpose in targetspace y @@ -163,12 +108,7 @@ public double getHorizontalDistanceByCT() { return horDis; } - /** - * Not yet experimented. Return shooter to goal angle degree by calculating with - * tx and ty. - * - * @return shooter to goal angle (degree) - */ + public double getHorDistanceByCal() { double pitch = getTy(); double yaw = getTx(); @@ -179,11 +119,7 @@ public double getHorDistanceByCal() { return horDistance; } - /** - * Gets the tag's pose in 2 dimension - * - * @return tagPose - */ + public Pose2d getTagPose2d() { if (getTv() == 1) { Optional tag_Pose3d = layout.getTagPose((int) getTID()); @@ -194,11 +130,7 @@ public Pose2d getTagPose2d() { } } - /** - * Gets the tag's pose in 3 dimension - * - * @return tagPose - */ + public Pose3d getTagPose3d() { if (getTv() == 1) { Optional tag_Pose3d = layout.getTagPose((int) getTID()); From a5a643db123fdfc00f2d67d0ff27d7baed76ad35 Mon Sep 17 00:00:00 2001 From: capy0827 Date: Wed, 12 Feb 2025 20:59:27 +0800 Subject: [PATCH 06/23] style: styleCheck some modifications to pass styleCheck --- src/main/java/frc/robot/vision/TagTracking.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/vision/TagTracking.java b/src/main/java/frc/robot/vision/TagTracking.java index 39dd9973..17f66571 100644 --- a/src/main/java/frc/robot/vision/TagTracking.java +++ b/src/main/java/frc/robot/vision/TagTracking.java @@ -42,7 +42,7 @@ public void setCamMode() { if (isCamOn) { table.getEntry("camMode").setNumber(0); - * else { + } else { table.getEntry("camMode").setNumber(1); } } From c7d36a3349385be97653a7774cd8be16959efb4a Mon Sep 17 00:00:00 2001 From: capy0827 Date: Wed, 12 Feb 2025 23:49:32 +0800 Subject: [PATCH 07/23] =?UTF-8?q?fix:=20TagTracking=20=E6=8A=8AgetCT()?= =?UTF-8?q?=E5=8A=A0=E5=9B=9E=E5=8E=BB?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/java/frc/robot/vision/TagTracking.java | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/vision/TagTracking.java b/src/main/java/frc/robot/vision/TagTracking.java index 17f66571..d497a20d 100644 --- a/src/main/java/frc/robot/vision/TagTracking.java +++ b/src/main/java/frc/robot/vision/TagTracking.java @@ -57,7 +57,10 @@ private void setPipeline(int pipeline) { } - * + public double[] getCT() { + ct = table.getEntry("camtran").getDoubleArray(new double[6]); + return ct; + } public double getTx() { tx = table.getEntry("tx").getDouble(0); return tx; @@ -67,7 +70,7 @@ public double getTx() { public double getTy() { ty = table.getEntry("ty").getDouble(0); - * eturn ty; + return ty; } @@ -77,7 +80,7 @@ public double getTv() { } - * + public double getTID() { id = table.getEntry("tid").getDouble(0); return id; @@ -88,7 +91,7 @@ public double[] getBT() { bt = table.getEntry("botpose_targetspace").getDoubleArray(new double[6]); return bt; } - * + public double getDistance() { From baa360770eb8c5759b75323841709543b27945a7 Mon Sep 17 00:00:00 2001 From: Hannahjjj97 Date: Thu, 13 Feb 2025 00:06:19 +0800 Subject: [PATCH 08/23] style: styleCheck some modifications to pass styleCheck --- .../java/frc/robot/vision/TagTracking.java | 23 +++++-------------- 1 file changed, 6 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/vision/TagTracking.java b/src/main/java/frc/robot/vision/TagTracking.java index d497a20d..c51346e0 100644 --- a/src/main/java/frc/robot/vision/TagTracking.java +++ b/src/main/java/frc/robot/vision/TagTracking.java @@ -41,8 +41,8 @@ public TagTracking() { public void setCamMode() { if (isCamOn) { table.getEntry("camMode").setNumber(0); - - } else { + + } else { table.getEntry("camMode").setNumber(1); } } @@ -51,49 +51,41 @@ private void setLedMode(int ledMode) { table.getEntry("ledMode").setNumber(ledMode); } - private void setPipeline(int pipeline) { table.getEntry("pipeline").setNumber(pipeline); } - public double[] getCT() { ct = table.getEntry("camtran").getDoubleArray(new double[6]); return ct; } + public double getTx() { tx = table.getEntry("tx").getDouble(0); return tx; } - public double getTy() { ty = table.getEntry("ty").getDouble(0); - - return ty; + + return ty; } - public double getTv() { tv = table.getEntry("tv").getDouble(0); return tv; } - - public double getTID() { id = table.getEntry("tid").getDouble(0); return id; } - public double[] getBT() { bt = table.getEntry("botpose_targetspace").getDoubleArray(new double[6]); return bt; - } - + } - public double getDistance() { // readValue(); double targetHeight = getBT()[1]; // botpose in targetspace y @@ -111,7 +103,6 @@ public double getHorizontalDistanceByCT() { return horDis; } - public double getHorDistanceByCal() { double pitch = getTy(); double yaw = getTx(); @@ -122,7 +113,6 @@ public double getHorDistanceByCal() { return horDistance; } - public Pose2d getTagPose2d() { if (getTv() == 1) { Optional tag_Pose3d = layout.getTagPose((int) getTID()); @@ -133,7 +123,6 @@ public Pose2d getTagPose2d() { } } - public Pose3d getTagPose3d() { if (getTv() == 1) { Optional tag_Pose3d = layout.getTagPose((int) getTID()); From cf97b5825b6d5be6dbb145703cdca46d98b4467c Mon Sep 17 00:00:00 2001 From: Hannahjjj97 Date: Thu, 13 Feb 2025 00:08:04 +0800 Subject: [PATCH 09/23] style: styleCheck some modifications to pass styleCheck --- src/main/java/frc/robot/vision/TagTracking.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/vision/TagTracking.java b/src/main/java/frc/robot/vision/TagTracking.java index c51346e0..53288a71 100644 --- a/src/main/java/frc/robot/vision/TagTracking.java +++ b/src/main/java/frc/robot/vision/TagTracking.java @@ -55,7 +55,7 @@ private void setPipeline(int pipeline) { table.getEntry("pipeline").setNumber(pipeline); } - public double[] getCT() { + public double[] getCt() { ct = table.getEntry("camtran").getDoubleArray(new double[6]); return ct; } @@ -76,7 +76,7 @@ public double getTv() { return tv; } - public double getTID() { + public double getTid() { id = table.getEntry("tid").getDouble(0); return id; } From 1f0dd80e4e2f4fd1e38f0bf536ef81a887ed0dec Mon Sep 17 00:00:00 2001 From: Hannahjjj97 Date: Thu, 13 Feb 2025 00:11:29 +0800 Subject: [PATCH 10/23] style: styleCheck some modifications to pass styleCheck --- .../java/frc/robot/vision/TagTracking.java | 20 ++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/vision/TagTracking.java b/src/main/java/frc/robot/vision/TagTracking.java index 53288a71..e7a76a88 100644 --- a/src/main/java/frc/robot/vision/TagTracking.java +++ b/src/main/java/frc/robot/vision/TagTracking.java @@ -81,24 +81,24 @@ public double getTid() { return id; } - public double[] getBT() { + public double[] getBt() { bt = table.getEntry("botpose_targetspace").getDoubleArray(new double[6]); return bt; } public double getDistance() { // readValue(); - double targetHeight = getBT()[1]; // botpose in targetspace y - double xDis = getBT()[0]; - double zDis = getBT()[2]; + double targetHeight = getBt()[1]; // botpose in targetspace y + double xDis = getBt()[0]; + double zDis = getBt()[2]; double horDis = Math.sqrt(Math.pow(xDis, 2) + Math.pow(zDis, 2)); distance = Math.sqrt(Math.pow(targetHeight, 2) + Math.pow(horDis, 2)); return distance; } - public double getHorizontalDistanceByCT() { + public double getHorizontalDistanceByCt() { double horDis = Math.sqrt( - (Math.pow(getCT()[2] + TagTrackingConstant.kCamToRampDistance, 2.0) + Math.pow(getCT()[0], 2.0))); + (Math.pow(getCt()[2] + TagTrackingConstant.kCamToRampDistance, 2.0) + Math.pow(getCt()[0], 2.0))); SmartDashboard.putNumber("Robottotagdistance", horDis); return horDis; } @@ -115,8 +115,10 @@ public double getHorDistanceByCal() { public Pose2d getTagPose2d() { if (getTv() == 1) { - Optional tag_Pose3d = layout.getTagPose((int) getTID()); - Pose2d tagPose2d = tag_Pose3d.isPresent() ? tag_Pose3d.get().toPose2d() : new Pose2d(); + Optional tag_Pose3d = layout.getTagPose((int) getTid()); + Pose2d tagPose2d = tag_Pose3d.isPresent() + ? tag_Pose3d.get().toPose2d() + : new Pose2d(); return tagPose2d; } else { return new Pose2d(); @@ -125,7 +127,7 @@ public Pose2d getTagPose2d() { public Pose3d getTagPose3d() { if (getTv() == 1) { - Optional tag_Pose3d = layout.getTagPose((int) getTID()); + Optional tag_Pose3d = layout.getTagPose((int) getTid()); Pose3d tagPose = tag_Pose3d.isPresent() ? tag_Pose3d.get() : new Pose3d(); return tagPose; } else { From 4c48866933bc5a898d5541f330761f7d3e3d80de Mon Sep 17 00:00:00 2001 From: Hannahjjj97 Date: Thu, 13 Feb 2025 00:42:26 +0800 Subject: [PATCH 11/23] style: styleCheck some modifications to pass styleCheck --- .../java/frc/robot/vision/TagTracking.java | 20 +++++++++++-------- 1 file changed, 12 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/vision/TagTracking.java b/src/main/java/frc/robot/vision/TagTracking.java index e7a76a88..93cd0f7d 100644 --- a/src/main/java/frc/robot/vision/TagTracking.java +++ b/src/main/java/frc/robot/vision/TagTracking.java @@ -88,17 +88,21 @@ public double[] getBt() { public double getDistance() { // readValue(); + // CHECKSTYLE.OFF: LocalVariableName double targetHeight = getBt()[1]; // botpose in targetspace y double xDis = getBt()[0]; double zDis = getBt()[2]; double horDis = Math.sqrt(Math.pow(xDis, 2) + Math.pow(zDis, 2)); distance = Math.sqrt(Math.pow(targetHeight, 2) + Math.pow(horDis, 2)); return distance; + // CHECKSTYLE.ON: LocalVariableName } public double getHorizontalDistanceByCt() { double horDis = Math.sqrt( - (Math.pow(getCt()[2] + TagTrackingConstant.kCamToRampDistance, 2.0) + Math.pow(getCt()[0], 2.0))); + (Math.pow(getCt()[2] + + TagTrackingConstant.kCamToRampDistance, 2.0) + + Math.pow(getCt()[0], 2.0))); SmartDashboard.putNumber("Robottotagdistance", horDis); return horDis; } @@ -115,9 +119,9 @@ public double getHorDistanceByCal() { public Pose2d getTagPose2d() { if (getTv() == 1) { - Optional tag_Pose3d = layout.getTagPose((int) getTid()); - Pose2d tagPose2d = tag_Pose3d.isPresent() - ? tag_Pose3d.get().toPose2d() + Optional tagPose3d = layout.getTagPose((int) getTid()); + Pose2d tagPose2d = tagPose3d.isPresent() + ? tagPose3d.get().toPose2d() : new Pose2d(); return tagPose2d; } else { @@ -127,8 +131,8 @@ public Pose2d getTagPose2d() { public Pose3d getTagPose3d() { if (getTv() == 1) { - Optional tag_Pose3d = layout.getTagPose((int) getTid()); - Pose3d tagPose = tag_Pose3d.isPresent() ? tag_Pose3d.get() : new Pose3d(); + Optional tagPose3d = layout.getTagPose((int) getTid()); + Pose3d tagPose = tagPose3d.isPresent() ? tagPose3d.get() : new Pose3d(); return tagPose; } else { return new Pose3d(); @@ -137,8 +141,8 @@ public Pose3d getTagPose3d() { public Pose2d getDesiredTagPose2d(double index) { if (getTv() == 1) { - Optional tag_Pose3d = layout.getTagPose((int) index); - Pose2d tagPose2d = tag_Pose3d.isPresent() ? tag_Pose3d.get().toPose2d() : new Pose2d(); + Optional tagPose3d = layout.getTagPose((int) index); + Pose2d tagPose2d = tagPose3d.isPresent() ? tagPose3d.get().toPose2d() : new Pose2d(); return tagPose2d; } else { return new Pose2d(); From f46ba4eacc0b7b4eeea945ecd7cfd17cfb01b035 Mon Sep 17 00:00:00 2001 From: Hannahjjj97 Date: Thu, 13 Feb 2025 00:49:44 +0800 Subject: [PATCH 12/23] =?UTF-8?q?fix:=20remove=20unused=20thing=20?= =?UTF-8?q?=E5=88=AA=E4=BA=86=E7=94=A8=E4=B8=8D=E5=88=B0=E7=9A=84=E6=9D=B1?= =?UTF-8?q?=E8=A5=BF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/java/frc/robot/Constants.java | 8 -------- .../java/frc/robot/vision/TagTracking.java | 20 ------------------- 2 files changed, 28 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0a21656d..ebb86946 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -26,12 +26,4 @@ public static final class PowerDistributionConstant { public static final double kClimberMotorMaxCurrent = 40; public static final double kRampMotorMaxCurrent = 40; } - - public static final class TagTrackingConstant { - public static final double kRampHeight = 0.0; - public static final double kCamHeight = 0.615; - public static final double kCamPitch = 10.0; - public static final double kCamToRampDistance = 0.11; - } - } diff --git a/src/main/java/frc/robot/vision/TagTracking.java b/src/main/java/frc/robot/vision/TagTracking.java index 93cd0f7d..b3d36ca5 100644 --- a/src/main/java/frc/robot/vision/TagTracking.java +++ b/src/main/java/frc/robot/vision/TagTracking.java @@ -7,7 +7,6 @@ import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc.robot.Constants.TagTrackingConstant; import java.io.IOException; import java.util.Optional; @@ -98,25 +97,6 @@ public double getDistance() { // CHECKSTYLE.ON: LocalVariableName } - public double getHorizontalDistanceByCt() { - double horDis = Math.sqrt( - (Math.pow(getCt()[2] - + TagTrackingConstant.kCamToRampDistance, 2.0) - + Math.pow(getCt()[0], 2.0))); - SmartDashboard.putNumber("Robottotagdistance", horDis); - return horDis; - } - - public double getHorDistanceByCal() { - double pitch = getTy(); - double yaw = getTx(); - double y = TagTrackingConstant.kCamHeight - * (1 / Math.tan(Math.toRadians(pitch + TagTrackingConstant.kCamPitch))); - double x = y * Math.tan(Math.toRadians(yaw)); - double horDistance = Math.sqrt(Math.pow(y, 2.0) + Math.pow(x, 2.0)); - return horDistance; - } - public Pose2d getTagPose2d() { if (getTv() == 1) { Optional tagPose3d = layout.getTagPose((int) getTid()); From e9aa00f0a8fe884359644e3a243594cd0f866e70 Mon Sep 17 00:00:00 2001 From: Hannahjjj97 Date: Thu, 13 Feb 2025 00:50:13 +0800 Subject: [PATCH 13/23] style: remove unused thing remove unused import --- src/main/java/frc/robot/vision/TagTracking.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/vision/TagTracking.java b/src/main/java/frc/robot/vision/TagTracking.java index b3d36ca5..37f9205a 100644 --- a/src/main/java/frc/robot/vision/TagTracking.java +++ b/src/main/java/frc/robot/vision/TagTracking.java @@ -6,7 +6,6 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import java.io.IOException; import java.util.Optional; From 710f22e19cb6aa726c0cf880d5fa9f91daa2c241 Mon Sep 17 00:00:00 2001 From: Hannahjjj97 Date: Thu, 13 Feb 2025 00:53:57 +0800 Subject: [PATCH 14/23] =?UTF-8?q?feat:=20=E4=BF=AE=E6=94=B9=E4=BA=86?= =?UTF-8?q?=E4=B8=80=E4=BA=9B=E6=9D=B1=E8=A5=BF=20=E7=9B=B8=E6=A9=9F?= =?UTF-8?q?=E5=90=8D=E5=AD=97=E6=9B=B4=E6=AD=A3=EF=BC=8C=E4=B8=8D=E7=94=A8?= =?UTF-8?q?=E8=AE=8A=E6=95=B8=E5=AD=98=20camMod?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/java/frc/robot/vision/TagTracking.java | 11 +---------- 1 file changed, 1 insertion(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/vision/TagTracking.java b/src/main/java/frc/robot/vision/TagTracking.java index 37f9205a..485ce851 100644 --- a/src/main/java/frc/robot/vision/TagTracking.java +++ b/src/main/java/frc/robot/vision/TagTracking.java @@ -25,7 +25,7 @@ public class TagTracking { private boolean isCamOn = true; public TagTracking() { - table = NetworkTableInstance.getDefault().getTable("limelight"); + table = NetworkTableInstance.getDefault().getTable("lyly"); setCamMode(); setLedMode(0); setPipeline(0); @@ -36,15 +36,6 @@ public TagTracking() { } } - public void setCamMode() { - if (isCamOn) { - table.getEntry("camMode").setNumber(0); - - } else { - table.getEntry("camMode").setNumber(1); - } - } - private void setLedMode(int ledMode) { table.getEntry("ledMode").setNumber(ledMode); } From a7fe211c55a21010697dbbee0eba5a80be17eb9e Mon Sep 17 00:00:00 2001 From: Hannahjjj97 Date: Thu, 13 Feb 2025 00:57:11 +0800 Subject: [PATCH 15/23] =?UTF-8?q?fix:=20remove=20error=20=E6=8A=8A?= =?UTF-8?q?=E5=87=BA=E9=8C=AF=E7=9A=84=E5=88=AA=E6=8E=89?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/java/frc/robot/vision/TagTracking.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/vision/TagTracking.java b/src/main/java/frc/robot/vision/TagTracking.java index 485ce851..d6445ded 100644 --- a/src/main/java/frc/robot/vision/TagTracking.java +++ b/src/main/java/frc/robot/vision/TagTracking.java @@ -26,7 +26,6 @@ public class TagTracking { public TagTracking() { table = NetworkTableInstance.getDefault().getTable("lyly"); - setCamMode(); setLedMode(0); setPipeline(0); try { From 7035b228e9a49383633f872153e6a3e6e47e716a Mon Sep 17 00:00:00 2001 From: capy0827 Date: Thu, 13 Feb 2025 12:57:13 +0800 Subject: [PATCH 16/23] =?UTF-8?q?style:=20TagTracking=20=E6=8A=8A=E4=B8=80?= =?UTF-8?q?=E4=BA=9B=E6=9D=B1=E8=A5=BF=E5=88=AA=E6=8E=89?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../java/frc/robot/vision/TagTracking.java | 56 +++++-------------- 1 file changed, 15 insertions(+), 41 deletions(-) diff --git a/src/main/java/frc/robot/vision/TagTracking.java b/src/main/java/frc/robot/vision/TagTracking.java index d6445ded..e1d5435c 100644 --- a/src/main/java/frc/robot/vision/TagTracking.java +++ b/src/main/java/frc/robot/vision/TagTracking.java @@ -5,7 +5,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.NetworkTableInstance; import java.io.IOException; import java.util.Optional; @@ -13,19 +13,8 @@ public class TagTracking { private final NetworkTable table; private final AprilTagFieldLayout layout; - private double tv; - private double tx; - private double ty; - private double id; - - private double[] bt; // botpose_targetspace - private double[] ct; // camerapose_targetspace - - private double distance; - private boolean isCamOn = true; - public TagTracking() { - table = NetworkTableInstance.getDefault().getTable("lyly"); + table = NetworkTableInstance.getDefault().getTable("limelight"); setLedMode(0); setPipeline(0); try { @@ -35,6 +24,8 @@ public TagTracking() { } } + + private void setLedMode(int ledMode) { table.getEntry("ledMode").setNumber(ledMode); } @@ -44,54 +35,49 @@ private void setPipeline(int pipeline) { } public double[] getCt() { - ct = table.getEntry("camtran").getDoubleArray(new double[6]); + double[] ct = table.getEntry("camtran").getDoubleArray(new double[6]); return ct; } public double getTx() { - tx = table.getEntry("tx").getDouble(0); + double tx = table.getEntry("tx").getDouble(0); return tx; } public double getTy() { - ty = table.getEntry("ty").getDouble(0); - + double ty = table.getEntry("ty").getDouble(0); return ty; } public double getTv() { - tv = table.getEntry("tv").getDouble(0); + double tv = table.getEntry("tv").getDouble(0); return tv; } public double getTid() { - id = table.getEntry("tid").getDouble(0); + double id = table.getEntry("tid").getDouble(0); return id; } public double[] getBt() { - bt = table.getEntry("botpose_targetspace").getDoubleArray(new double[6]); + double[] bt = table.getEntry("botpose_targetspace").getDoubleArray(new double[6]); return bt; } public double getDistance() { // readValue(); - // CHECKSTYLE.OFF: LocalVariableName + // CHECKSTYLE.OFF:LocalVariableName double targetHeight = getBt()[1]; // botpose in targetspace y double xDis = getBt()[0]; double zDis = getBt()[2]; double horDis = Math.sqrt(Math.pow(xDis, 2) + Math.pow(zDis, 2)); - distance = Math.sqrt(Math.pow(targetHeight, 2) + Math.pow(horDis, 2)); + double distance = Math.sqrt(Math.pow(targetHeight, 2) + Math.pow(horDis, 2)); return distance; - // CHECKSTYLE.ON: LocalVariableName } - public Pose2d getTagPose2d() { if (getTv() == 1) { - Optional tagPose3d = layout.getTagPose((int) getTid()); - Pose2d tagPose2d = tagPose3d.isPresent() - ? tagPose3d.get().toPose2d() - : new Pose2d(); + Optional tag_Pose3d = layout.getTagPose((int) getTid()); + Pose2d tagPose2d = tag_Pose3d.isPresent() ? tag_Pose3d.get().toPose2d() : new Pose2d(); return tagPose2d; } else { return new Pose2d(); @@ -103,7 +89,7 @@ public Pose3d getTagPose3d() { Optional tagPose3d = layout.getTagPose((int) getTid()); Pose3d tagPose = tagPose3d.isPresent() ? tagPose3d.get() : new Pose3d(); return tagPose; - } else { + } else { return new Pose3d(); } } @@ -117,16 +103,4 @@ public Pose2d getDesiredTagPose2d(double index) { return new Pose2d(); } } - - public void isVisionOn() { - isCamOn = !isCamOn; - } - - public void setAutoCamOn() { - table.getEntry("camMode").setNumber(0); - } - - public void setAutoCamOff() { - table.getEntry("camMode").setNumber(1); - } } From c8a13a70fe108f2ffeb7d3d72f02ce91ac65cde7 Mon Sep 17 00:00:00 2001 From: Hannahjjj97 Date: Thu, 13 Feb 2025 17:20:42 +0800 Subject: [PATCH 17/23] =?UTF-8?q?style:=20remove=20space=20=E7=A7=BB?= =?UTF-8?q?=E9=99=A4=E6=9C=83=E9=80=A0=E6=88=90=E8=A1=9D=E7=AA=81=E7=9A=84?= =?UTF-8?q?=E7=A9=BA=E6=A0=BC?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/java/frc/robot/Constants.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ebb86946..599add33 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -26,4 +26,5 @@ public static final class PowerDistributionConstant { public static final double kClimberMotorMaxCurrent = 40; public static final double kRampMotorMaxCurrent = 40; } + } From b8b251e93f4b5f648a692fc2e9330a0cf4c7cb65 Mon Sep 17 00:00:00 2001 From: Hannahjjj97 Date: Thu, 13 Feb 2025 17:23:01 +0800 Subject: [PATCH 18/23] style: styleCheck disable styleCheck --- src/main/java/frc/robot/vision/TagTracking.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/vision/TagTracking.java b/src/main/java/frc/robot/vision/TagTracking.java index e1d5435c..767729aa 100644 --- a/src/main/java/frc/robot/vision/TagTracking.java +++ b/src/main/java/frc/robot/vision/TagTracking.java @@ -66,13 +66,15 @@ public double[] getBt() { public double getDistance() { // readValue(); - // CHECKSTYLE.OFF:LocalVariableName + // CHECKSTYLE.OFF: LocalVariableName double targetHeight = getBt()[1]; // botpose in targetspace y double xDis = getBt()[0]; double zDis = getBt()[2]; double horDis = Math.sqrt(Math.pow(xDis, 2) + Math.pow(zDis, 2)); double distance = Math.sqrt(Math.pow(targetHeight, 2) + Math.pow(horDis, 2)); return distance; + // CHECKSTYLE.ON: LocalVariableName + } public Pose2d getTagPose2d() { if (getTv() == 1) { From 882d81545e35579ed8b8d9ec49ba8e85a673bf53 Mon Sep 17 00:00:00 2001 From: Hannahjjj97 Date: Thu, 13 Feb 2025 17:23:45 +0800 Subject: [PATCH 19/23] style: styleCheck auto formatting --- src/main/java/frc/robot/vision/TagTracking.java | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/vision/TagTracking.java b/src/main/java/frc/robot/vision/TagTracking.java index 767729aa..778b016c 100644 --- a/src/main/java/frc/robot/vision/TagTracking.java +++ b/src/main/java/frc/robot/vision/TagTracking.java @@ -5,7 +5,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.NetworkTableInstance; import java.io.IOException; import java.util.Optional; @@ -24,8 +24,6 @@ public TagTracking() { } } - - private void setLedMode(int ledMode) { table.getEntry("ledMode").setNumber(ledMode); } @@ -74,8 +72,9 @@ public double getDistance() { double distance = Math.sqrt(Math.pow(targetHeight, 2) + Math.pow(horDis, 2)); return distance; // CHECKSTYLE.ON: LocalVariableName - } + + public Pose2d getTagPose2d() { if (getTv() == 1) { Optional tag_Pose3d = layout.getTagPose((int) getTid()); @@ -91,7 +90,7 @@ public Pose3d getTagPose3d() { Optional tagPose3d = layout.getTagPose((int) getTid()); Pose3d tagPose = tagPose3d.isPresent() ? tagPose3d.get() : new Pose3d(); return tagPose; - } else { + } else { return new Pose3d(); } } From df7101bb64476ab054450d5ba0ffe7532f13fdc1 Mon Sep 17 00:00:00 2001 From: Hannahjjj97 Date: Thu, 13 Feb 2025 17:25:25 +0800 Subject: [PATCH 20/23] style: styleCheck some modifications to pass styleCheck --- src/main/java/frc/robot/vision/TagTracking.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/vision/TagTracking.java b/src/main/java/frc/robot/vision/TagTracking.java index 778b016c..b079a38d 100644 --- a/src/main/java/frc/robot/vision/TagTracking.java +++ b/src/main/java/frc/robot/vision/TagTracking.java @@ -77,8 +77,8 @@ public double getDistance() { public Pose2d getTagPose2d() { if (getTv() == 1) { - Optional tag_Pose3d = layout.getTagPose((int) getTid()); - Pose2d tagPose2d = tag_Pose3d.isPresent() ? tag_Pose3d.get().toPose2d() : new Pose2d(); + Optional tagPose3d = layout.getTagPose((int) getTid()); + Pose2d tagPose2d = tagPose3d.isPresent() ? tagPose3d.get().toPose2d() : new Pose2d(); return tagPose2d; } else { return new Pose2d(); From 3eb2db22755a8e5154c64a1cbcf242c37ac4a816 Mon Sep 17 00:00:00 2001 From: Hannahjjj97 Date: Thu, 13 Feb 2025 18:35:35 +0800 Subject: [PATCH 21/23] =?UTF-8?q?style:=20=E7=B0=A1=E5=8C=96=20command=20?= =?UTF-8?q?=E6=8A=8A=E9=87=8D=E8=A4=87=20command=20=E7=B0=A1=E5=8C=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/java/frc/robot/vision/TagTracking.java | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/vision/TagTracking.java b/src/main/java/frc/robot/vision/TagTracking.java index b079a38d..a1c4de4b 100644 --- a/src/main/java/frc/robot/vision/TagTracking.java +++ b/src/main/java/frc/robot/vision/TagTracking.java @@ -76,13 +76,7 @@ public double getDistance() { public Pose2d getTagPose2d() { - if (getTv() == 1) { - Optional tagPose3d = layout.getTagPose((int) getTid()); - Pose2d tagPose2d = tagPose3d.isPresent() ? tagPose3d.get().toPose2d() : new Pose2d(); - return tagPose2d; - } else { - return new Pose2d(); - } + getTagPose3d().toPose2d(); } public Pose3d getTagPose3d() { From f7ca8c7ba1f887574fc02901faa2767cbe54ed49 Mon Sep 17 00:00:00 2001 From: Hannahjjj97 Date: Thu, 13 Feb 2025 18:38:10 +0800 Subject: [PATCH 22/23] =?UTF-8?q?fix:=20=E6=B2=92=E5=AF=AB=20return=20?= =?UTF-8?q?=E6=8A=8A=20return=20=E8=A3=9C=E4=BA=86=E4=B8=8A=E5=8E=BB?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/java/frc/robot/vision/TagTracking.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/vision/TagTracking.java b/src/main/java/frc/robot/vision/TagTracking.java index a1c4de4b..ec09b46d 100644 --- a/src/main/java/frc/robot/vision/TagTracking.java +++ b/src/main/java/frc/robot/vision/TagTracking.java @@ -76,7 +76,7 @@ public double getDistance() { public Pose2d getTagPose2d() { - getTagPose3d().toPose2d(); + return getTagPose3d().toPose2d(); } public Pose3d getTagPose3d() { From 905b7fa3b797b8577515772f6bc34c4d0ed4e0d2 Mon Sep 17 00:00:00 2001 From: Hannahjjj97 Date: Thu, 13 Feb 2025 19:18:35 +0800 Subject: [PATCH 23/23] =?UTF-8?q?refactor:=20=E6=8A=8A=E9=87=8D=E8=A4=87?= =?UTF-8?q?=E7=9A=84=E7=A8=8B=E5=BC=8F=E7=A2=BC=E5=8E=BB=E6=8E=89=20?= =?UTF-8?q?=E8=AE=8A=E6=88=90=E4=B8=80=E5=80=8B=E7=A5=9E=E5=A5=87=E7=9A=84?= =?UTF-8?q?=E6=9D=B1=E8=A5=BF=E4=B8=80=E7=9B=B4=E8=BD=89=E6=8F=9B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/java/frc/robot/vision/TagTracking.java | 15 ++++++--------- 1 file changed, 6 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/vision/TagTracking.java b/src/main/java/frc/robot/vision/TagTracking.java index ec09b46d..02352904 100644 --- a/src/main/java/frc/robot/vision/TagTracking.java +++ b/src/main/java/frc/robot/vision/TagTracking.java @@ -73,15 +73,18 @@ public double getDistance() { return distance; // CHECKSTYLE.ON: LocalVariableName } - public Pose2d getTagPose2d() { return getTagPose3d().toPose2d(); } public Pose3d getTagPose3d() { + return getDesiredTagPose3d(getTid()); + } + + public Pose3d getDesiredTagPose3d(double index) { if (getTv() == 1) { - Optional tagPose3d = layout.getTagPose((int) getTid()); + Optional tagPose3d = layout.getTagPose((int) index); Pose3d tagPose = tagPose3d.isPresent() ? tagPose3d.get() : new Pose3d(); return tagPose; } else { @@ -90,12 +93,6 @@ public Pose3d getTagPose3d() { } public Pose2d getDesiredTagPose2d(double index) { - if (getTv() == 1) { - Optional tagPose3d = layout.getTagPose((int) index); - Pose2d tagPose2d = tagPose3d.isPresent() ? tagPose3d.get().toPose2d() : new Pose2d(); - return tagPose2d; - } else { - return new Pose2d(); - } + return getDesiredTagPose3d(index).toPose2d(); } }