From b236f43f4d9051497d6f8953040feca3975b9efe Mon Sep 17 00:00:00 2001
From: Alyssum-chenyiyi <13162501939@163.com>
Date: Thu, 5 Mar 2026 18:57:57 +0800
Subject: [PATCH] climb auto

---
 src/main/java/frc/robot/Constants.java        |   3 +
 .../subsystems/CommandSwerveDrivetrain.java   |  70 ++++++++++++
 .../robot/subsystems/LimelightSupplier.java   | 108 ++++++++++++++++++
 3 files changed, 181 insertions(+)
 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..1b6c31d 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 double AutoClimbAlignSpeed = 0.5;//等查资料
+        public static final double AutoClimbAlignTolerance = 0.1;//？
+        public static final double AutoClimbmaxAngularVelocity  = 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..c7f845e 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;
@@ -130,6 +131,8 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
         );
     }
 
+    
+
 
 
         
@@ -276,6 +279,73 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
 
         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;
+                break;
+            }
+        }
+        if(!ifRightTag){
+            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.AutoClimbAlignTolerance){
+            tx = 0;
+        }
+        else{
+            tx *= multiply ;
+        }
+        
+        if(Math.abs(targetTz)<Constants.Limelight.AutoClimbAlignTolerance){
+            targetTz = 0;
+        }
+        else{
+            targetTz*= multiply ;
+            targetTz*= 5;
+        }
+
+        if(Math.abs(ry)<Constants.Limelight.AutoClimbAlignTolerance){
+            ry = 0;
+        }
+        else{
+            ry/=10;
+            ry = Math.min(ry, Constants.Limelight.AutoClimbmaxAngularVelocity);
+            ry = Math.max(ry, -Constants.Limelight.AutoClimbmaxAngularVelocity);
+        }
+        
+        
+        positioningNetworkTable.getEntry("interestPointsClimb tx").setDouble(tx);
+        positioningNetworkTable.getEntry("interestPointsClimb targetTz").setDouble(targetTz);
+        positioningNetworkTable.getEntry("interestPointsClimb ry").setDouble(ry);
+        
+
+        ChassisSpeeds scaled = new ChassisSpeeds(
+            tx,                 // X 不变
+            targetTz,                 // Y 不变
+            ry
+        );
+        driveRobotRelative(scaled);
+        // drive(
+        //     new Translation2d(tx, targetTz),
+        //     ry,
+        //     false,
+        //     false
+        // );
+
+        return "Driving to AprilTag";
+    }
     @Override
     public void periodic() {
 
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..5d4ec72
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/LimelightSupplier.java
@@ -0,0 +1,108 @@
+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("LimelightTA", getTA());
+            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);
+        }
+        
+    }
+}