climber

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() {