admin6494/FRC6494_2026Regional-offseason
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

for limelight

Browse Source
This commit is contained in:
OnDYCLaptop
2026-03-05 16:48:24 +08:00
parent 118fdc4086
commit ebcf6ff3c3
3 changed files with 193 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc/robot/Constants.java
+3
View File
@@ -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 {
src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java
+73 -2
View File
@@ -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;
src/main/java/frc/robot/subsystems/LimelightSupplier.java
+117
View File
@@ -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);
}
}
}