From 6fe9613b67fc4e656c1b090c8201518da9855243 Mon Sep 17 00:00:00 2001 From: Alyssum-chenyiyi <13162501939@163.com> Date: Fri, 6 Mar 2026 19:40:10 +0800 Subject: [PATCH] climber limelight 0306 --- src/main/java/frc/robot/Constants.java | 25 ++- .../java/frc/robot/controls/Autocommand.java | 2 +- .../frc/robot/controls/DriveControls.java | 2 + .../subsystems/CommandSwerveDrivetrain.java | 177 +++++++++++------- .../robot/subsystems/LimelightSupplier.java | 2 +- 5 files changed, 140 insertions(+), 68 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b12add4..2e90fcb 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -80,9 +80,28 @@ public final class Constants { public static final Boolean UsingMetaTag2 = true; public static final int climbPipelineIndex = 8; public static final int locatePipelineIndex = 0; - public static final double AutoClimbAlignSpeed = 0.5;//等查资料 - public static final double AutoClimbAlignTolerance = 0.1;//？ - public static final double AutoClimbmaxAngularVelocity = 0.5;//等查资料 + + // 误差死区 / 对齐判定 + public static final double AutoClimbToleranceTX = 1.5; // deg + public static final double AutoClimbToleranceTZ = 0.10; // m + public static final double AutoClimbToleranceRY = 2.0; // deg + + // P 控制系数 + public static final double AutoClimbKpTX = 0.020; // deg -> rad/s + public static final double AutoClimbKpTZ = 0.90; // m -> m/s + public static final double AutoClimbKpRY = 0.015; // deg -> rad/s + + // 最小输出（防止快到目标时推不动） + public static final double AutoClimbMinVX = 0.18; // m/s + public static final double AutoClimbMinOmega = 0.10; // rad/s + + // 最大输出（防止冲太猛） + public static final double AutoClimbMaxVX = 1.20; // m/s + public static final double AutoClimbMaxOmega = 0.60; // rad/s + + // 远距离 / 大角度时额外限速，可后续微调 + public static final double AutoClimbFastTurnThresholdTX = 15.0; // deg + public static final double AutoClimbFastTurnOmega = 0.45; // rad/s } public class Field { diff --git a/src/main/java/frc/robot/controls/Autocommand.java b/src/main/java/frc/robot/controls/Autocommand.java index 22ad966..a1d9342 100644 --- a/src/main/java/frc/robot/controls/Autocommand.java +++ b/src/main/java/frc/robot/controls/Autocommand.java @@ -76,7 +76,7 @@ import frc.robot.Constants.RobotStatus; ).until(() -> drivetrain.isAlignedToAprilTag()) ); } - + } diff --git a/src/main/java/frc/robot/controls/DriveControls.java b/src/main/java/frc/robot/controls/DriveControls.java index 171b45b..a53fc9b 100644 --- a/src/main/java/frc/robot/controls/DriveControls.java +++ b/src/main/java/frc/robot/controls/DriveControls.java @@ -135,10 +135,12 @@ public class DriveControls { vomega = 0; break; case PassingBall: + LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex); vomega = calculateRotationSpeedFromRotationAngle(calculateDistanceAndRotationToPassBall()[1]); autoControlNetworkTable.getEntry("autoRotationRate").setDouble(vomega); break; case AutoAimming: + LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex); vomega = calculateRotationSpeedFromRotationAngle(distanceAndRotation[1]); autoControlNetworkTable.getEntry("autoRotationRate").setDouble(vomega); break; diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index a431bc9..e63c6e8 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -27,6 +27,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Subsystem; import frc.robot.Constants.DriveMode; import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain; +import frc.robot.subsystems.LimelightHelpers.RawFiducial; import frc.robot.Constants; import edu.wpi.first.networktables.*; @@ -269,81 +270,131 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su // 2) 转太快：拒绝（角速度你可以从 Pigeon2 或 drivetrain state 拿） double omegaDegPerSec = getPigeon2().getAngularVelocityZWorld().getValueAsDouble(); // 若你有这个信号 if (Math.abs(omegaDegPerSec) > kMaxOmegaDegPerSec) { - System.out.println("NoUsingMetaTag2ForTooFastRotationSpeed"); + //System.out.println("NoUsingMetaTag2ForTooFastRotationSpeed"); return false; } // 3) 跳变太大：拒绝（Phoenix6 推荐的稳定策略） //Pose2d current = getState().Pose; if (currentPose.getTranslation().getDistance(mt2.pose.getTranslation()) > kMaxVisionJumpMeters) { - System.out.println("NoUsingMetaTag2ForTooChangingPose"); + //System.out.println("NoUsingMetaTag2ForTooChangingPose"); return false; } return true; } - public String driveToAprilTag(){ - //Rotation2d targetHeading = getPose().getRotation(); - if(!LimelightSupplier.isTargetVisible() ){ - return "No Target"; - } - boolean ifRightTag = false; - int trueId = LimelightSupplier.getAprilTagID(); - for(int id : new int[]{16,32}){ - if(id == trueId){ - ifRightTag = true; - System.out.println("Target get."); - break; - } - } - if(!ifRightTag){ - System.out.println("No Target."); - return "No Target"; - } - double tx = LimelightSupplier.getTX() ; - double targetTz = -LimelightSupplier.getTargetTZ(); - double ry = LimelightSupplier.getTargetRotationY(); - - double multiply = Constants.Limelight.AutoClimbAlignSpeed; - if(Math.abs(tx) Constants.Limelight.AutoClimbToleranceTZ) { + vx = tz * Constants.Limelight.AutoClimbKpTZ; + + // 最小输出，避免快到目标时不动 + if (Math.abs(vx) < Constants.Limelight.AutoClimbMinVX) { + vx = Math.copySign(Constants.Limelight.AutoClimbMinVX, vx); + } + + // 最大输出限幅 + vx = clamp(vx, + -Constants.Limelight.AutoClimbMaxVX, + Constants.Limelight.AutoClimbMaxVX); + } + + // 2) 旋转控制：tx 为主，ry 为辅 + // tx 负责“镜头中心对准 tag” + // ry 负责“机器人姿态微调” + double omegaFromTx = 0.0; + double omegaFromRy = 0.0; + + if (Math.abs(tx) > Constants.Limelight.AutoClimbToleranceTX) { + omegaFromTx = -tx * Constants.Limelight.AutoClimbKpTX; + } + + if (Math.abs(ry) > Constants.Limelight.AutoClimbToleranceRY) { + omegaFromRy = ry * Constants.Limelight.AutoClimbKpRY; + } + + omega = omegaFromTx + omegaFromRy; + + // 大偏角时直接给较明显的转速，先把朝向拉回来 + if (Math.abs(tx) > Constants.Limelight.AutoClimbFastTurnThresholdTX) { + omega = Math.copySign(Constants.Limelight.AutoClimbFastTurnOmega, omega); + } + + // 最小输出，避免转不动 + if (Math.abs(omega) > 1e-6 && Math.abs(omega) < Constants.Limelight.AutoClimbMinOmega) { + omega = Math.copySign(Constants.Limelight.AutoClimbMinOmega, omega); + } + + // 最大输出限幅 + omega = clamp(omega, + -Constants.Limelight.AutoClimbMaxOmega, + Constants.Limelight.AutoClimbMaxOmega); + + // ===== 调试输出 ===== + driveNetworkTable.getEntry("ClimbTX").setDouble(tx); + driveNetworkTable.getEntry("ClimbTZ").setDouble(tz); + driveNetworkTable.getEntry("ClimbRY").setDouble(ry); + driveNetworkTable.getEntry("ClimbVXCmd").setDouble(vx); + driveNetworkTable.getEntry("ClimbOmegaCmd").setDouble(omega); + driveNetworkTable.getEntry("ClimbAligned").setBoolean(isAlignedToAprilTag); + + // ===== 下发到底盘 ===== + driveRobotRelative(new ChassisSpeeds(vx, vy, omega)); + + return "Driving to AprilTag"; +} + public boolean isAlignedToAprilTag(){ return isAlignedToAprilTag; } diff --git a/src/main/java/frc/robot/subsystems/LimelightSupplier.java b/src/main/java/frc/robot/subsystems/LimelightSupplier.java index 5d4ec72..7836b78 100644 --- a/src/main/java/frc/robot/subsystems/LimelightSupplier.java +++ b/src/main/java/frc/robot/subsystems/LimelightSupplier.java @@ -17,7 +17,7 @@ import frc.robot.subsystems.LimelightHelpers.RawFiducial; public class LimelightSupplier extends SubsystemBase { public LimelightSupplier() { - setPipeline(1); + setPipeline(8); } public static double getTX() {