From c9fbbcc65ca136a564fd496a948c54dae48089e6 Mon Sep 17 00:00:00 2001 From: HarryWangOnLaptop Date: Mon, 23 Feb 2026 11:33:00 +0800 Subject: [PATCH] =?UTF-8?q?limelight=E7=B2=BE=E5=BA=A6=E4=B8=8D=E9=AB=98?= =?UTF-8?q?=20=E4=BD=86=E6=98=AF=E8=83=BD=E7=94=A8=E4=BA=86?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/java/frc/robot/Constants.java | 1 + .../subsystems/CommandSwerveDrivetrain.java | 28 +++++++++++++++---- 2 files changed, 24 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f17ac1e..3dfc4f3 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -67,6 +67,7 @@ public final class Constants { public class Limelight { public static final String LIMELIGHT_NAME_Shooter = "limelight-shooter"; public static final String LIMELIGHT_NAME_Intaker = "limelight-intaker"; + public static final Boolean UsingMetaTag2 = false; } public class Field { diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 82be7a6..acaaabf 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -74,8 +74,8 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su SmartDashboard.putData("Field", m_field); SmartDashboard.putData("PositioningShooterll", shooter_ll_field); SmartDashboard.putData("PositioningIntakerll", intaker_ll_field); - LimelightHelpers.SetIMUMode(Constants.Limelight.LIMELIGHT_NAME_Shooter, 4); - LimelightHelpers.SetIMUMode(Constants.Limelight.LIMELIGHT_NAME_Intaker, 4); + LimelightHelpers.SetIMUMode(Constants.Limelight.LIMELIGHT_NAME_Shooter, 1); + LimelightHelpers.SetIMUMode(Constants.Limelight.LIMELIGHT_NAME_Intaker, 1); positioningNetworkTable = NetworkTableInstance.getDefault().getTable("Positioning"); driveNetworkTable = NetworkTableInstance.getDefault().getTable("Drive"); ll_shooter_NT = NetworkTableInstance.getDefault().getTable("Positioning/limelight-shooter"); @@ -170,10 +170,10 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su Pose2d current = getState().Pose; LimelightHelpers.PoseEstimate mt2_shooter = - LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.Limelight.LIMELIGHT_NAME_Shooter); + getLimelightPoseEstimate(Constants.Limelight.LIMELIGHT_NAME_Shooter); LimelightHelpers.PoseEstimate mt2_intaker = - LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.Limelight.LIMELIGHT_NAME_Intaker); + getLimelightPoseEstimate(Constants.Limelight.LIMELIGHT_NAME_Intaker); boolean shooterAvailable = ifLimelightPositioningAvailable(mt2_shooter, current); boolean intakerAvailable = ifLimelightPositioningAvailable(mt2_intaker, current); @@ -191,11 +191,15 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su return; }else if (shooterAvailable && !intakerAvailable) { + double shooterDistance = current.getTranslation().getDistance(mt2_shooter.pose.getTranslation()); + ll_shooter_NT.getEntry("Distance").setDouble(shooterDistance); positioningNetworkTable.getEntry("MetaTag2Available").setBoolean(true); positioningNetworkTable.getEntry("Usingll").setString("Shooter"); //positioningNetworkTable.getEntry("MetaTag2DistanceDifference").setDouble(Double.NaN); mt2_final = mt2_shooter; }else if (!shooterAvailable && intakerAvailable) { + double intakerDistance = current.getTranslation().getDistance(mt2_intaker.pose.getTranslation()); + ll_intaker_NT.getEntry("Distance").setDouble(intakerDistance); positioningNetworkTable.getEntry("MetaTag2Available").setBoolean(true); positioningNetworkTable.getEntry("Usingll").setString("Intaker"); //positioningNetworkTable.getEntry("MetaTag2DistanceDifference").setDouble(Double.NaN); @@ -205,10 +209,13 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su double shooterDistance = current.getTranslation().getDistance(mt2_shooter.pose.getTranslation()); double intakerDistance = current.getTranslation().getDistance(mt2_intaker.pose.getTranslation()); double difference = mt2_shooter.pose.getTranslation().getDistance(mt2_intaker.pose.getTranslation()); + ll_shooter_NT.getEntry("Distance").setDouble(shooterDistance); ll_intaker_NT.getEntry("Distance").setDouble(intakerDistance); positioningNetworkTable.getEntry("MetaTag2DistanceDifference").setDouble(difference); - positioningNetworkTable.getEntry("Usingll").setString(shooterDistance <= intakerDistance ? "Shooter" : "Intaker"); + positioningNetworkTable.getEntry("MetaTag2XDifference").setDouble(Math.abs(mt2_shooter.pose.getX() - mt2_intaker.pose.getX())); + positioningNetworkTable.getEntry("MetaTag2YDifference").setDouble(Math.abs(mt2_shooter.pose.getY() - mt2_intaker.pose.getY())); + positioningNetworkTable.getEntry("Usingll").setString(shooterDistance <= intakerDistance ? "ChosenShooter" : "ChosenIntaker"); mt2_final = shooterDistance <= intakerDistance ? mt2_shooter : mt2_intaker; } @@ -234,6 +241,12 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su positioningNetworkTable.getEntry("MetaTag2Available").setBoolean(true); } + private LimelightHelpers.PoseEstimate getLimelightPoseEstimate(String limelightName) { + if (Constants.Limelight.UsingMetaTag2) { + return LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelightName); + } + return LimelightHelpers.getBotPoseEstimate_wpiBlue(limelightName); + } private boolean ifLimelightPositioningAvailable(LimelightHelpers.PoseEstimate mt2, Pose2d currentPose) { // 1) 没看到 tag：拒绝 if (mt2 == null || mt2.tagCount == 0) { @@ -272,6 +285,11 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su pose.getRotation().getDegrees(), // 关键：要符合 wpiBlue 约定 0, 0, 0, 0, 0 ); + LimelightHelpers.SetRobotOrientation( + Constants.Limelight.LIMELIGHT_NAME_Intaker, + pose.getRotation().getDegrees(), // 关键：要符合 wpiBlue 约定 + 0, 0, 0, 0, 0 + ); fuseLimelightMT2();