Merge branch '1' of https://github.com/HarryWang163/FRC6494_2026Regional into 1

src/main/java/frc/robot/Constants.java

@@ -15,35 +15,30 @@ public final class Constants {
         CrossingTrench
     }
     public class Shooter {
-        public static final double conveyorSpeed = 20;
+        public static final double conveyorSpeed =25;
         public static final Slot0Configs flyWheelSlot0Configs = new Slot0Configs()
             .withKP(0.0).withKI(0).withKD(0)
-            .withKV(0.0088).withKA(0.0).withKS(0.0);
+            .withKV(0.099).withKA(0.0).withKS(0.0);
 
         public static final Slot0Configs conveyorSlot0Configs = new Slot0Configs()
             .withKP(0.0).withKI(0).withKD(0)
-            .withKV(0.02).withKA(0.15).withKS(0);
+            .withKV(0.16).withKA(0.15).withKS(0);
 
         public static final Slot0Configs backboardSlot0Configs = new Slot0Configs()
             .withKP(0.01).withKI(0).withKD(0)
-            .withKV(0.008).withKA(0.0).withKS(0.010);
+            .withKV(0.03).withKA(0.0).withKS(0.010);
 
         public class backboardPositionPID {
-            public static final double kP = 0.1;
+            public static final double kP = 0.3;
             public static final double kI = 0.0;
             public static final double kD = 0.0;
             
         }
-        public static final double backboardSpeedMax = 30; //也就是output
+        public static final double backboardSpeedMax = 60; //也就是output
         // 背板的上下限位角度
         public static final double backboardUpLimit = 3000.0;  // 背板上限角度（根据实际需求设置）
         public static final double backboardDownLimit = 0.0;  // 背板下限角度（根据实际需求设置）
     }
-    public class ShooterCalculation{
-        public static final double positionNear = 0;  // 近距离背板角度
-        public static final double positionMid  = 1600;  // 中距离背板角度
-        public static final double postionFar  = 3200;  // 远距离背板角度
-    }
     public class AutoPositioning {
         public static final double autoRotationkP = 0.1;
         public static final double autoRotationForBumpTargetDegrees = 45;
@@ -83,14 +78,41 @@ 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 climbPipelineIndex = 8;
+        public static final int locatePipelineIndex = 0;
+
+        // 误差死区 / 对齐判定
+        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 {
         public static final double RedHubPositionX = 11.914;
         public static final double RedHubPositionY = 4.034;
-        public static final double PassingBallPosX = 0;
+        public static final double BlueHubPositionX = 4.625;
-        public static final double PassingBallPosY1 = 2.51;
+        public static final double BlueHubPositionY = 4.034;
-        public static final double PassingBallPosY2 = 5.556;
+        public static final double PassingBallPosY1 = 1.405;
+        public static final double PassingBallPosY2 = 6.664;
+        public static final double RedPassingBallPosX = 13.355;
+        public static final double BluePassingBallPosX = 3.184;
     }
     
 
@@ -111,12 +133,12 @@ public final class Constants {
         .withKV(0.00).withKA(0.00).withKS(0.00);
     //如果 rotater 受重力影响明显，可能需要用 withKG/withGravityType/withGravityArmPositionOffset（看机构形式）(这句fromAI)
 
-    public static final double IntakeGetterSpeedforAuto = 0.0;
+    public static final double IntakeGetterSpeedforAuto = 0.4;
     // 单位RPS
     public static final double positionToleranceRotations = 0.00;
     //rotater容差
     public static final double IntakeGetterK = 0.3; // intake getter速度与触发器输入的比例
-    public static final double Getterminspeed = 0.2; // intake getter的基础速度（即触发器输入为0时的速度，正值表示默认向内转）
+    public static final double Getterminspeed = 0.3; // intake getter的基础速度（即触发器输入为0时的速度，正值表示默认向内转）
     }   
     public static class Climber {
 

src/main/java/frc/robot/RobotContainer.java

@@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
@@ -58,7 +59,7 @@ public class RobotContainer {
   private final DriveControls driveControls = new DriveControls(drivetrain, controllerlower, robotStatusManager );
   private final ShooterControls shooterControls = new ShooterControls(shooterSubsystem, controllerupper,robotStatusManager);
   private final ClimberControls climberControls = new ClimberControls(climberSubsystem, controllerupper);
-  private final IntakerControls intakerControls = new IntakerControls(IntakerSubsystem, controllerupper);
+  private final IntakerControls intakerControls = new IntakerControls(IntakerSubsystem, shooterSubsystem, controllerupper);
   private final DriveGainsTuner driveGainsTuner = new DriveGainsTuner(drivetrain);
   private ConfigTalonFXMotorTuner configFlywheelTuner = new ConfigTalonFXMotorTuner(shooterSubsystem.flywheelMotorLeft, "flywheel", Constants.Shooter.flyWheelSlot0Configs);
   private ConfigTalonFXMotorTuner configConveyorTuner = new ConfigTalonFXMotorTuner(shooterSubsystem.conveyorMotor, "conveyor", Constants.Shooter.conveyorSlot0Configs);
@@ -118,9 +119,19 @@ public class RobotContainer {
     // Start：强行使用metaTag2全场定位
     controllerlower.start().whileTrue(drivetrain.run(drivetrain::forceUsingLimelightMT2));
 
-    //LM 按下时autoaimming
+    //LM 按下时autoaimming，松开时alltelop
-    controllerlower.leftStick().onTrue(robotStatusManager.setStatusCommand(RobotStatus.AutoAimming));
+    // controllerlower.leftStick().onTrue(robotStatusManager.setStatusCommand(RobotStatus.AutoAimming));
-    controllerlower.leftStick().onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
+    // controllerlower.leftStick().onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
+
+    //双操均可触发autoaimming模式，松开时alltelop
+    Trigger autoAim = controllerupper.rightBumper().or(controllerlower.rightStick());
+    autoAim.onTrue(robotStatusManager.setStatusCommand(RobotStatus.AutoAimming));
+    autoAim.onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
+
+    //双操均可触发passingball模式，松开时alltelop
+    Trigger autoAim2 = controllerupper.leftBumper().or(controllerlower.leftStick());
+    autoAim2.onTrue(robotStatusManager.setStatusCommand(RobotStatus.PassingBall));
+    autoAim2.onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
 
     controllerlower.leftTrigger()
     .onTrue(robotStatusManager.setStatusCommand(RobotStatus.CrossingTrench))
@@ -129,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(){

src/main/java/frc/robot/controls/Autocommand.java

@@ -1,5 +1,6 @@
 package frc.robot.controls;
 
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -11,6 +12,7 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.robot.subsystems.ClimberSubsystem;
 import frc.robot.RobotStatusManager;
 import frc.robot.subsystems.IntakerSubsystem;
+import frc.robot.subsystems.LimelightSupplier;
 import frc.robot.subsystems.ShooterSubsystem;
 import frc.robot.subsystems.CommandSwerveDrivetrain;
 import frc.robot.Constants;
@@ -58,6 +60,24 @@ import frc.robot.Constants.RobotStatus;
                 intaker.stopIntakerotater();;  // 停止旋转
             }, intaker));
         }
+        public static Command autoAlignToClimb(CommandSwerveDrivetrain drivetrain) {
+        return Commands.sequence(
+            new InstantCommand(() -> {
+                LimelightSupplier.setPipeline(Constants.Limelight.climbPipelineIndex);
+            }),
+            Commands.runEnd(
+                () -> {
+                    drivetrain.driveToAprilTag();
+                },
+                () -> {
+                    LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex);
+                },
+                drivetrain
+            ).until(() -> drivetrain.isAlignedToAprilTag())
+        );
+    }
+    
+
 
     }
 

src/main/java/frc/robot/controls/ClimberControls.java

@@ -1,11 +1,16 @@
 package frc.robot.controls;
 
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.robot.Constants;
 import frc.robot.subsystems.ClimberSubsystem;
+import frc.robot.subsystems.LimelightSupplier;
 
 public class ClimberControls {
 
@@ -53,4 +58,7 @@ public class ClimberControls {
     public Command resetClimberEncoderCommand(ClimberSubsystem climber) {
         return Commands.runOnce(climber::zeroEncoder, climber);
     }
+
+    
+    
 }

src/main/java/frc/robot/controls/DriveControls.java

@@ -19,12 +19,14 @@ import frc.robot.Constants.DriveMode;
 import frc.robot.Constants.RobotStatus;
 import frc.robot.generated.TunerConstants;
 import frc.robot.subsystems.CommandSwerveDrivetrain;
+import frc.robot.subsystems.LimelightSupplier;
 
 import java.util.Optional;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance; 
 
 public class DriveControls {
+
     public boolean isRedAlliance;
 
     private final CommandSwerveDrivetrain drivetrain;
@@ -122,6 +124,8 @@ public class DriveControls {
         // 将距离值写入 NetworkTable
         autoControlNetworkTable.getEntry("distanceToHub").setDouble(distanceAndRotation[0]);
         autoControlNetworkTable.getEntry("angleDifferenceToHub").setDouble(distanceAndRotation[1]);
+        autoControlNetworkTable.getEntry("distanceToPassball").setDouble(calculateDistanceAndRotationToPassBall()[0]);
+        autoControlNetworkTable.getEntry("angleDifferenceToPassball").setDouble(calculateDistanceAndRotationToPassBall()[1]);
         
         switch (robotStatusManager.getStatus()) {
             case Stopped:
@@ -130,10 +134,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;
@@ -141,6 +147,7 @@ public class DriveControls {
                 autoControlNetworkTable.getEntry("autoRotationRate").setDouble(Double.NaN);
                 break;
             case Climbing:
+                alignToClimb();
                 break;
             case CrossingBump:
                 double differenceBump = calculateDifferenceToTwoTarget(drivetrain.getState().Pose.getY(), Constants.AutoPositioning.bumpY[0], Constants.AutoPositioning.bumpY[1]);
@@ -152,6 +159,7 @@ public class DriveControls {
                         Math.max(Math.abs(raw), 0.4),
                         raw
                     );
+                    vy = isRedAlliance ? -vy : vy;
                 }
                 autoControlNetworkTable.getEntry("BumpDifference").setDouble(differenceBump);
 
@@ -181,6 +189,7 @@ public class DriveControls {
                         Math.max(Math.abs(raw), 0.4),
                         raw
                     );
+                    vy = isRedAlliance ? -vy : vy;
                 }
                 autoControlNetworkTable.getEntry("TrenchDifference").setDouble(differenceTrench);
                 Rotation2d currentAngle = drivetrain.getState().Pose.getRotation();
@@ -204,7 +213,8 @@ public class DriveControls {
         if (fieldCentricEnabled || isAutoLike) {
 
     // 自动状态用 BlueAlliance（不翻转），手柄 field-centric 仍用原 fieldCentric（OperatorPerspective）
-        var chosen = isAutoLike ? fieldCentricAuto : fieldCentric;
+        //var chosen = isAutoLike ? fieldCentricAuto : fieldCentric;
+        var chosen = fieldCentric;
         return chosen
                 .withVelocityX(vx)
                 .withVelocityY(vy)
@@ -233,8 +243,8 @@ public class DriveControls {
     }
     public double[] calculateDistanceAndRotationToHub() {
         Pose2d currentPose = drivetrain.getState().Pose; // 获取机器人当前的位置和角度
-        double targetX = Constants.Field.RedHubPositionX;  // 目标 X 坐标
+        double targetX = isRedAlliance ? Constants.Field.RedHubPositionX : Constants.Field.BlueHubPositionX;
-        double targetY = Constants.Field.RedHubPositionY;  // 目标 Y 坐标
+        double targetY = isRedAlliance ? Constants.Field.RedHubPositionY : Constants.Field.BlueHubPositionY;
 
         // 计算目标角度（相对于场地坐标系）
         double deltaX = targetX - currentPose.getX();
@@ -261,7 +271,7 @@ public class DriveControls {
     }
     public double[] calculateDistanceAndRotationToPassBall() {
         Pose2d currentPose = drivetrain.getState().Pose; // 获取机器人当前的位置和角度
-        double targetX = Constants.Field.PassingBallPosX;  // 目标 X 坐标
+        double targetX = isRedAlliance ? Constants.Field.RedPassingBallPosX : Constants.Field.BluePassingBallPosX;  // 目标 X 坐标
         double targetY1 = Constants.Field.PassingBallPosY1;  // 目标 Y 坐标
         double targetY2 = Constants.Field.PassingBallPosY2;  // 目标 Y 坐标
        
@@ -389,6 +399,17 @@ public class DriveControls {
     }
     public Command getAllianceColorCommand() {
         return Commands.runOnce(this::setTeamColors);
-}
+    }
+    public void alignToClimb(){
+        LimelightSupplier.setPipeline(Constants.Limelight.climbPipelineIndex);
+        if(!drivetrain.isAlignedToAprilTag()){
+            drivetrain.driveToAprilTag();
+        }
+        else{
+            LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex);
+            drivetrain.stop();
+        }
+    }
+
 
 }

src/main/java/frc/robot/controls/IntakerControls.java

@@ -6,16 +6,19 @@ import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.subsystems.IntakerSubsystem;
+import frc.robot.subsystems.ShooterSubsystem;
 import frc.robot.Constants;
 
 
 public class IntakerControls {
     private final IntakerSubsystem IntakerSubsystem;
     private final CommandXboxController controller;
+    private final ShooterSubsystem ShooterSubsystem;
 
     //private static final double ROTATER_STEP_PER_CYCLE = 0.00;
 
-    public IntakerControls(IntakerSubsystem Intaker, CommandXboxController controller) {
+    public IntakerControls(IntakerSubsystem Intaker, ShooterSubsystem Shooter, CommandXboxController controller) {
+        this.ShooterSubsystem = Shooter;
         this.IntakerSubsystem = Intaker;
         this.controller = controller;
     }
@@ -69,10 +72,15 @@ public class IntakerControls {
     
     public Command defaultIntakerCommand() {
         return Commands.run(() -> {
-        double getterspeed = controller.getLeftTriggerAxis() * Constants.Intaker.IntakeGetterK + Constants.Intaker.Getterminspeed;
+        double getterspeed = 0;
-        if (controller.getLeftTriggerAxis() < 0.1) {
+        double conveyoyspeedforintake = 15;
-            getterspeed = 0;
+        double statorCurrent = ShooterSubsystem.getConveyorStatorCurrent();
+        
+        if (controller.getLeftTriggerAxis() > 0.1) {
+            getterspeed = controller.getLeftTriggerAxis() * Constants.Intaker.IntakeGetterK + Constants.Intaker.Getterminspeed;
+            ShooterSubsystem.setConveyorSpeedByRPS(conveyoyspeedforintake);
             }
+
         IntakerSubsystem.setIntakerGetterSpeed(getterspeed);
         
         if(controller.getLeftY() < -0.5){

src/main/java/frc/robot/controls/ShooterControls.java

@@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.Constants;
 import frc.robot.RobotStatusManager;
 import frc.robot.Constants.Intaker;
+import frc.robot.Constants.RobotStatus;
 import frc.robot.subsystems.IntakerSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
 
@@ -37,7 +38,7 @@ public class ShooterControls {
                     break;
                 case AllTelop:
                 case PassingBall:
-                    var xP = shooterSubsystem.getDistance();
+                    var xP = shooterSubsystem.getDistanceToPassball();
                     //xP = Math.max(1.2, Math.min(4.5, xP));
                     flywheelSpeed = 0.6956596811733782*xP*xP*xP*xP*xP*xP*xP-14.164408860183267*xP*xP*xP*xP*xP*xP+119.7115375240918*xP*xP*xP*xP*xP-543.0984876677477*xP*xP*xP*xP+1425.1928543240738*xP*xP*xP-2156.199296490315*xP*xP+1736.568139600292*xP-516.097598057351;
                     flywheelSpeed += flywheelSpeedOffset;
@@ -48,12 +49,9 @@ public class ShooterControls {
                         flywheelSpeed = 0;
                         conveyorSpeed = 0;
                     }
-                    if(shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble()<flywheelSpeed*0.7){
+                    if(shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble()<flywheelSpeed*0.95){
                         conveyorSpeed = 0;
                     }
-                    // flywheelSpeed = calculateFlywheelSpeedOnlywithOffset(60);
-                    // double flywheelSpeed = calculateFlywheelSpeed(0);
-                    // double BackboardPosition = calculateBackboardPosition(0);
                     shooterControlTable.getEntry("flywheelTargetSpeed").setDouble(flywheelSpeed);
                     shooterControlTable.getEntry("conveyerTargetSpeed").setDouble(conveyorSpeed);
                     shooterControlTable.getEntry("backboardTargetPosition").setDouble(backboardPosition);
@@ -67,7 +65,7 @@ public class ShooterControls {
                     }  
                     break;
                 case AutoAimming:
-                    var x = shooterSubsystem.getDistance();
+                    var x = shooterSubsystem.getDistanceToHub();
                     x = Math.max(1.2, Math.min(4.5, x));
                     flywheelSpeed = 0.6956596811733782*x*x*x*x*x*x*x-14.164408860183267*x*x*x*x*x*x+119.7115375240918*x*x*x*x*x-543.0984876677477*x*x*x*x+1425.1928543240738*x*x*x-2156.199296490315*x*x+1736.568139600292*x-516.097598057351;
                     flywheelSpeed += flywheelSpeedOffset;
@@ -78,17 +76,14 @@ public class ShooterControls {
                         flywheelSpeed = 0;
                         conveyorSpeed = 0;
                     }
-                    if(shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble()<flywheelSpeed*0.7){
+                    if(shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble()<flywheelSpeed*0.95){
                         conveyorSpeed = 0;
                     }
-                    // flywheelSpeed = calculateFlywheelSpeedOnlywithOffset(60);
-                    // double flywheelSpeed = calculateFlywheelSpeed(0);
-                    // double BackboardPosition = calculateBackboardPosition(0);
                     shooterControlTable.getEntry("flywheelTargetSpeed").setDouble(flywheelSpeed);
                     shooterControlTable.getEntry("conveyerTargetSpeed").setDouble(conveyorSpeed);
                     shooterControlTable.getEntry("backboardTargetPosition").setDouble(backboardPosition);
                     shooterSubsystem.setFlywheelSpeedByRPS(flywheelSpeed);
-                    shooterSubsystem.setConveyorSpeedByRPS(conveyorSpeed);
+                    shooterSubsystem.setConveyorSpeedOpen(5);
                     shooterSubsystem.setBackboardPosition(backboardPosition);  
                     if(shooterSubsystem.isBackboardAtTarget()){
                         shooterSubsystem.backboardMotor.set(0);

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;
@@ -26,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.*;
@@ -46,6 +48,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
     private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.k180deg;
     /* Keep track if we've ever applied the operator perspective before or not */
     private boolean m_hasAppliedOperatorPerspective = false;
+    private boolean isAlignedToAprilTag = false;
 
     private DriveMode m_driveMode = DriveMode.OTHER;
     
@@ -63,6 +66,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
     private Field2d shooter_ll_field;
     private Field2d intaker_ll_field;
 
+
     public CommandSwerveDrivetrain(
         SwerveDrivetrainConstants drivetrainConstants,
         SwerveModuleConstants<?, ?, ?>... modules
@@ -130,8 +134,11 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
         ); 
     }
     
-
+    public void stop() {
-
+        setControl(
+            new SwerveRequest.ApplyRobotSpeeds().withSpeeds(new ChassisSpeeds(0, 0, 0))
+        ); 
+    }
 
     private static final double kMaxVisionJumpMeters = 1.0;
     private static final double kMaxOmegaDegPerSec = 360.0;
@@ -263,19 +270,134 @@ 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;
     }
+    private double clamp(double value, double min, double max) {
+        return Math.max(min, Math.min(max, value));
+    }
+
+    public String driveToAprilTag() {
+        System.out.println("Pipeline = " + LimelightHelpers.getCurrentPipelineIndex(Constants.Limelight.LIMELIGHT_NAME_Intaker));
+        System.out.println("TV = " + LimelightSupplier.isTargetVisible());
+
+    if (!LimelightSupplier.isTargetVisible()) {
+        isAlignedToAprilTag = false;
+        //stop();
+        return "No Target";
+    }
+
+    RawFiducial[] fiducials =
+        LimelightHelpers.getRawFiducials(Constants.Limelight.LIMELIGHT_NAME_Intaker);
+
+    boolean foundDesiredTag = false;
+    for (RawFiducial fiducial : fiducials) {
+        if (fiducial.id == 16 || fiducial.id == 32) {
+            foundDesiredTag = true;
+            break;
+        }
+    }
+
+    if (!foundDesiredTag) {
+        isAlignedToAprilTag = false;
+        stop();
+        return "No Desired Tag";
+    }
+
+    // ===== 读取原始误差 =====
+    double tx = LimelightSupplier.getTX();                 // deg
+    double tz = -LimelightSupplier.getTargetTZ();          // m，方向按你当前经验保留负号
+    double ry = LimelightSupplier.getTargetRotationY();    // deg
+
+    // ===== 用原始误差判断是否已对齐 =====
+    isAlignedToAprilTag =
+        Math.abs(tx) < Constants.Limelight.AutoClimbToleranceTX &&
+        Math.abs(tz) < Constants.Limelight.AutoClimbToleranceTZ &&
+        Math.abs(ry) < Constants.Limelight.AutoClimbToleranceRY;
+
+    if (isAlignedToAprilTag) {
+        stop();
+        return "Aligned";
+    }
+
+    // ===== 计算输出 =====
+    double vx = 0.0;
+    double vy = 0.0;
+    double omega = 0.0;
+
+    // 1) 前后控制：根据 tz 前进/后退
+    if (Math.abs(tz) > 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;
+    }
     @Override
     public void periodic() {
 
@@ -287,7 +409,11 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
         driveNetworkTable.getEntry("YawFromState").setDouble(getState().Pose.getRotation().getDegrees());
         driveNetworkTable.getEntry("DriveMode").setString(m_driveMode.name());
         driveNetworkTable.getEntry("IsFieldCentric").setBoolean(m_driveMode == DriveMode.FIELD_CENTRIC);
-
+        positioningNetworkTable.getEntry("isAlignedToAprilTag").setBoolean(isAlignedToAprilTag());
+        positioningNetworkTable.getEntry("interestPointsClimber TX").setDouble(LimelightSupplier.getTX());
+        positioningNetworkTable.getEntry("interestPointsClimber RY").setDouble(LimelightSupplier.getTargetRotationY());
+        positioningNetworkTable.getEntry("interestPointsClimber TZ").setDouble(-LimelightSupplier.getTargetTZ());
+        positioningNetworkTable.getEntry("interestPointsCl").setBoolean(false);
         // 1) 每帧喂给 LL：机器人当前 yaw（度），其余先全 0
         LimelightHelpers.SetRobotOrientation(
             Constants.Limelight.LIMELIGHT_NAME_Shooter,

src/main/java/frc/robot/subsystems/IntakerSubsystem.java

@@ -83,26 +83,9 @@ public class IntakerSubsystem extends SubsystemBase{
         stopIntakerotater();
     }
     public void IntakerDownNonStop(){
-        double currentPosition = getIntakerotaterPosition();
+        intakerotater.set(-0.05);
-        if (currentPosition > Constants.Intaker.intakeRotaterDownLimit) {
-        
-        intakerotater.set(-0.05);}
-        // double currentPosition = getIntakerotaterPosition();
-        // if (currentPosition > Constants.Intaker.intakeRotaterDownLimit) {
-        //     intakerotater.setControl(velocityRequest.withVelocity(Constants.Intaker.intakeRotaterVelocity));
-        // } else {
-        //     stopIntakerotater();
-        // }
         }
     public void IntakerUpNonStop(){
-        double currentPosition = getIntakerotaterPosition();
+        intakerotater.set(0.1);
-        if (currentPosition < Constants.Intaker.intakeRotaterUpLimit) {
-        intakerotater.set(0.2);}
-        
-        // if (currentPosition < Constants.Intaker.intakeRotaterUpLimit) {
-        //     intakerotater.setControl(velocityRequest.withVelocity(-Constants.Intaker.intakeRotaterVelocity));
-        // } else {
-        //     stopIntakerotater();
-        // }
     }
 }

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(8);
+    }
+
+    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);
+        }
+        
+    }
+}

src/main/java/frc/robot/subsystems/ShooterSubsystem.java

@@ -4,6 +4,7 @@ import com.ctre.phoenix6.configs.MotorOutputConfigs;
 import com.ctre.phoenix6.configs.Slot0Configs;
 import com.ctre.phoenix6.controls.Follower;
 import com.ctre.phoenix6.controls.VelocityDutyCycle;
+import com.ctre.phoenix6.controls.VelocityVoltage;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.hardware.TalonFXS;
 import com.ctre.phoenix6.signals.InvertedValue;
@@ -31,7 +32,7 @@ public class ShooterSubsystem extends SubsystemBase {
 
     private NetworkTable shooterNetworkTable;
 
-    private final VelocityDutyCycle velocityRequest = new VelocityDutyCycle(0);
+    private final VelocityVoltage velocityRequest = new VelocityVoltage(0);
 
 
     // 初始化各个部件
@@ -94,9 +95,17 @@ public class ShooterSubsystem extends SubsystemBase {
     public void setConveyorSpeedByRPS(double speed) {
         conveyorMotor.setControl(velocityRequest.withVelocity(speed));  // 设置传动电机速度
     }
+    public void setConveyorSpeedOpen(double speed){
+        conveyorMotor.set(speed);
+    }
 
     //调试用
     public void setBackboardSpeedByRPS(double speed){
+         if (speed > 0 && speed < 10) {
+        speed = 10;
+        } else if (speed < 0 && speed > -10) {
+        speed = -10;
+        }
         backboardMotor.setControl(velocityRequest.withVelocity(speed));
     }
 
@@ -149,6 +158,10 @@ public class ShooterSubsystem extends SubsystemBase {
     public void resetbackboardencoder(){
         backboardEncoder.reset();
     }
+    public double getConveyorStatorCurrent(){
+        double statorCurrent = conveyorMotor.getStatorCurrent().getValueAsDouble();
+        return statorCurrent;
+    }
 
     @Override
     public void periodic() {
@@ -160,11 +173,16 @@ public class ShooterSubsystem extends SubsystemBase {
         shooterNetworkTable.getEntry("backboardCurrentRate").setDouble(getBackboardPosition());
         shooterNetworkTable.getEntry("backboardTargetRate").setDouble(backboardPID.getSetpoint().position);
         shooterNetworkTable.getEntry("isBackboardAtTarget").setBoolean(isBackboardAtTarget());
-        shooterNetworkTable.getEntry("distanceGetted").setDouble(getDistance());
+        shooterNetworkTable.getEntry("distanceGetted").setDouble(getDistanceToHub());
+        shooterNetworkTable.getEntry("distancetopassball").setDouble(getDistanceToPassball());
+        shooterNetworkTable.getEntry("statorCurrent").setDouble(getConveyorStatorCurrent());
     }
 
-    public double getDistance(){
+    public double getDistanceToHub(){
         //获得车子距离Hub的距离
         return NetworkTableInstance.getDefault().getTable("AutoControl").getEntry("distanceToHub").getDouble(0.0);
     }
+    public double getDistanceToPassball(){
+        return NetworkTableInstance.getDefault().getTable("AutoControl").getEntry("distanceToPassball").getDouble(0.0);
+    }
 }