From ebcf6ff3c3ad92e87f85d71a668f8ccd6a2b54c3 Mon Sep 17 00:00:00 2001 From: OnDYCLaptop Date: Thu, 5 Mar 2026 16:48:24 +0800 Subject: [PATCH] for limelight --- src/main/java/frc/robot/Constants.java | 3 + .../subsystems/CommandSwerveDrivetrain.java | 75 ++++++++++- .../robot/subsystems/LimelightSupplier.java | 117 ++++++++++++++++++ 3 files changed, 193 insertions(+), 2 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/LimelightSupplier.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8793572..a05d6dc 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -78,6 +78,9 @@ public final class Constants { public static final String LIMELIGHT_NAME_Shooter = "limelight-shooter"; public static final String LIMELIGHT_NAME_Intaker = "limelight-intaker"; public static final Boolean UsingMetaTag2 = true; + public static final int Pipeline = 1; + public static final double TXDeadHand = 0.5; + public static final double TZDeadHand = 0.5; } public class Field { diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 46987f0..ffda559 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -15,6 +15,7 @@ import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; @@ -127,12 +128,82 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su setControl( new SwerveRequest.ApplyRobotSpeeds().withSpeeds(scaled) - ); + ); } + // public String driveToAprilTagOfClimber(double speed) { + // // 记录当前方向，作为目标方向，避免在驱动过程中偏航 + // Rotation2d TargetHeading=getPose().getRotation(); + // // 设置 Limelight pipeline（25赛季只有一条，直接设置） + // LimelightHelpers.setPipelineIndex(Constants.Limelight.LIMELIGHT_NAME_Intaker,Constants.Limelight.Pipeline); + // // 如果 Limelight 没有检测到目标，直接返回 + // if (!LimelightSupplier.isTargetVisible()) { + // return "No Target"; + // } + // // 获取检测到的 AprilTag ID + // int trueId = LimelightSupplier.getAprilTagID(); + + // // 只有特定的 Tag ID 被认为是目标 + // int[] validTagIDs = {16,32}; + // boolean isValidTag = false; + // for (int id : validTagIDs) { + // if (id == trueId) { + // isValidTag = true; + // break; + // } + // } + // if (!isValidTag) { + // return "No Target"; + // } + + // // 获取 Limelight 的偏移数据 + // double tx = LimelightSupplier.getTX(); // 水平偏移角度 + // double targetTz = -LimelightSupplier.getTargetTZ(); // 目标与底盘前方的距离，取负方便坐标系 + // double ry = LimelightSupplier.getTargetRotationY(); // 目标旋转角 + + // // 根据速度参数计算实际驱动速度 + // double multiply = Constants.Swerve.maxSpeed * speed; + + // // 水平偏移死区处理 + // if (Math.abs(tx) < Constants.Limelight.TXDeadHand) { + // tx = 0; + // } else { + // tx *= multiply; + // } + + // // 前进距离死区处理 + // if (Math.abs(targetTz) < Constants.Limelight.TZDeadHand) { + // targetTz = 0; + // } else { + // targetTz *= multiply; + // targetTz *= 5; // 放大系数，增加前进响应 + // } + + // // 旋转角度死区处理 + // if (Math.abs(ry) < Constants.Swerve.reefCorrection.roatationYDeadHand) { + // ry = 0; + // } else { + // ry /= 10; // 缩小旋转响应 + // ry = Math.min(ry, Constants.Swerve.maxAngularVelocity); + // ry = Math.max(ry, -Constants.Swerve.maxAngularVelocity); + // } + + // // 输出到 SmartDashboard 方便调试 + // SmartDashboard.putNumber("driveTx_r", tx); + // SmartDashboard.putNumber("driveTz_r", targetTz); + // SmartDashboard.putNumber("driveRy_r", ry); + + // // 调用底盘驱动方法 + // drive( + // new Translation2d(tx, targetTz), // x = 水平, y = 前进 + // ry, // 旋转速度 + // false, // 是否相对场地驱动 + // false // 是否速度保持为绝对速度 + // ); + // return "Driving to AprilTag"; + // } - private static final double kMaxVisionJumpMeters = 1.0; private static final double kMaxOmegaDegPerSec = 360.0; diff --git a/src/main/java/frc/robot/subsystems/LimelightSupplier.java b/src/main/java/frc/robot/subsystems/LimelightSupplier.java new file mode 100644 index 0000000..b2ed76e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/LimelightSupplier.java @@ -0,0 +1,117 @@ +package frc.robot.subsystems; + +import frc.robot.Constants; + +import java.lang.annotation.Target; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.wpilibj.GenericHID.RumbleType; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.LimelightHelpers; +import frc.robot.subsystems.LimelightHelpers.RawFiducial; + +public class LimelightSupplier extends SubsystemBase { + + public LimelightSupplier() { + setPipeline(1); + } + + public static double getTX() { + return LimelightHelpers.getTX(Constants.Limelight.LIMELIGHT_NAME_Intaker); + } + + public static double getTY() { + return LimelightHelpers.getTY(Constants.Limelight.LIMELIGHT_NAME_Intaker); + } + + public static double getTA() { + // Logic to get area from Limelight + return LimelightHelpers.getTA(Constants.Limelight.LIMELIGHT_NAME_Intaker); + } + + public static boolean isTargetVisible() { + if(!LimelightHelpers.getTV(Constants.Limelight.LIMELIGHT_NAME_Intaker)){ + return false; + } + // Logic to check if a target is visible + return LimelightHelpers.getTV(Constants.Limelight.LIMELIGHT_NAME_Intaker); + } + public static int getAprilTagID(){ + RawFiducial[] fiducials = LimelightHelpers.getRawFiducials(Constants.Limelight.LIMELIGHT_NAME_Intaker); + int id = -1; + for (RawFiducial fiducial : fiducials) { + id = fiducial.id; // 标签ID + } + return id; + } + public static void setPipeline(int pipeline) { + if(((int)LimelightHelpers.getCurrentPipelineIndex(Constants.Limelight.LIMELIGHT_NAME_Intaker)) != pipeline){ + LimelightHelpers.setPipelineIndex(Constants.Limelight.LIMELIGHT_NAME_Intaker, pipeline); + } + } + + public static double[] getTargetPose() { + // Logic to get the target pose from Limelight + return LimelightHelpers.getBotPose_TargetSpace(Constants.Limelight.LIMELIGHT_NAME_Intaker); + } + + public static Pose2d getBotPose2d_wpiBlue(){ + return LimelightHelpers.getBotPose2d_wpiBlue(Constants.Limelight.LIMELIGHT_NAME_Intaker); + } + public static double getTargetRotationY(){ + if(isTargetVisible()){ + return getTargetPose()[4]; + } + else{ + return 0.00; + } + } + public static double getTargetTZ(){ + if(isTargetVisible()){ + return getTargetPose()[2]; + } + else{ + return 0.00; + } + } + @Override + public void periodic() { + SmartDashboard.putNumber("LimelightPipeIndex", LimelightHelpers.getCurrentPipelineIndex(Constants.Limelight.LIMELIGHT_NAME_Intaker)); + SmartDashboard.putString("LimelightPipeType", LimelightHelpers.getCurrentPipelineType(Constants.Limelight.LIMELIGHT_NAME_Intaker)); + if (isTargetVisible()){ + SmartDashboard.putString("LimelightTargetVisible", "YES"); + // SmartDashboard.putNumber("LimelightTX", getTX()); + // SmartDashboard.putNumber("LimelightTY", getTY()); + SmartDashboard.putNumber("LimelightTA", getTA()); + // double[] targetPose = getTargetPose(); + // String s = ""; + // for(double x : targetPose){ + // s += Double.toString(x); + // s += " "; + // } + // SmartDashboard.putString("LimeLightTP", s); + SmartDashboard.putNumber("TargetRY", getTargetRotationY()); + SmartDashboard.putNumber("TargetTX", getTX()); + SmartDashboard.putNumber("TargetTZ", getTargetTZ()); + RawFiducial[] fiducials = LimelightHelpers.getRawFiducials(Constants.Limelight.LIMELIGHT_NAME_Intaker); + int id = -1; + for (RawFiducial fiducial : fiducials) { + id = fiducial.id; // 标签ID + } + SmartDashboard.putNumber("LimelightTargetID", id); + + } + else { + SmartDashboard.putString("LimelightTargetVisible", "NO"); + SmartDashboard.putNumber("TargetRY", 0); + SmartDashboard.putNumber("TargetTX", 0); + SmartDashboard.putNumber("TargetTZ", 0); + SmartDashboard.putNumber("LimelightTargetID", -1); + } + + } +}