From 0da9ee5f36c8ca510e6ad4ffb907d1bc5c1a3e72 Mon Sep 17 00:00:00 2001
From: OnDYCLaptop <dingdoorfiah@gamil.com>
Date: Thu, 5 Mar 2026 19:34:24 +0800
Subject: [PATCH] climber

---
 src/main/java/frc/robot/Constants.java        |  3 +
 src/main/java/frc/robot/RobotContainer.java   |  4 ++
 .../frc/robot/controls/DriveControls.java     |  1 +
 .../subsystems/CommandSwerveDrivetrain.java   | 68 ++++++++++++++++++-
 4 files changed, 75 insertions(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 06a788c..dcafbe3 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -81,6 +81,9 @@ public final class Constants {
         public static final int Pipeline = 1;
         public static final double TXDeadHand = 0.5;
         public static final double TZDeadHand = 0.5;
+        public static final double AutoClimbAlignSpeed = 0.1;
+        public static final double AutoClimbAlignTolerance = 0.1;
+        public static final double AutoClimbmaxAngularVelocity = 0.1;
     }
 
     public class Field {
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 1b6c243..c0a8a3d 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -140,6 +140,10 @@ public class RobotContainer {
     controllerlower.rightTrigger()
     .onTrue(robotStatusManager.setStatusCommand(RobotStatus.CrossingBump))
     .onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
+
+    controllerlower.a()
+    .onTrue(robotStatusManager.setStatusCommand(RobotStatus.Climbing))
+    .onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
   }
 
   private void configueShooter(){
diff --git a/src/main/java/frc/robot/controls/DriveControls.java b/src/main/java/frc/robot/controls/DriveControls.java
index b273d0a..7958cbd 100644
--- a/src/main/java/frc/robot/controls/DriveControls.java
+++ b/src/main/java/frc/robot/controls/DriveControls.java
@@ -145,6 +145,7 @@ public class DriveControls {
                 autoControlNetworkTable.getEntry("autoRotationRate").setDouble(Double.NaN);
                 break;
             case Climbing:
+                drivetrain.driveToAprilTag();
                 break;
             case CrossingBump:
                 double differenceBump = calculateDifferenceToTwoTarget(drivetrain.getState().Pose.getY(), Constants.AutoPositioning.bumpY[0], Constants.AutoPositioning.bumpY[1]);
diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java
index 3189e76..0813b5d 100644
--- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java
+++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java
@@ -130,7 +130,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
             new SwerveRequest.ApplyRobotSpeeds().withSpeeds(scaled)
         ); 
     }
-    
+
     private static final double kMaxVisionJumpMeters = 1.0;
     private static final double kMaxOmegaDegPerSec = 360.0;
 
@@ -274,6 +274,72 @@ 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() {