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

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

@@ -78,6 +78,8 @@ 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 AutoClimbAlignSpeed = 0.5;//等查资料
         public static final double AutoClimbAlignTolerance = 0.1;//？
         public static final double AutoClimbmaxAngularVelocity  = 0.5;//等查资料

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

@@ -20,6 +20,7 @@ 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;
@@ -145,7 +146,7 @@ public class DriveControls {
                 autoControlNetworkTable.getEntry("autoRotationRate").setDouble(Double.NaN);
                 break;
             case Climbing:
-                drivetrain.driveToAprilTag();
+                alignToClimb();
                 break;
             case CrossingBump:
                 double differenceBump = calculateDifferenceToTwoTarget(drivetrain.getState().Pose.getY(), Constants.AutoPositioning.bumpY[0], Constants.AutoPositioning.bumpY[1]);
@@ -398,5 +399,16 @@ 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/subsystems/CommandSwerveDrivetrain.java

@@ -47,6 +47,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;
     
@@ -64,6 +65,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
     private Field2d shooter_ll_field;
     private Field2d intaker_ll_field;
 
+
     public CommandSwerveDrivetrain(
         SwerveDrivetrainConstants drivetrainConstants,
         SwerveModuleConstants<?, ?, ?>... modules
@@ -131,6 +133,12 @@ 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;
 
@@ -324,8 +332,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
             ry = Math.min(ry, Constants.Limelight.AutoClimbmaxAngularVelocity);
             ry = Math.max(ry, -Constants.Limelight.AutoClimbmaxAngularVelocity);
         }
-        
-        
+        isAlignedToAprilTag = Math.abs(tx) < Constants.Limelight.AutoClimbAlignTolerance && Math.abs(targetTz) < Constants.Limelight.AutoClimbAlignTolerance && Math.abs(ry) < Constants.Limelight.AutoClimbAlignTolerance;
         ChassisSpeeds scaled = new ChassisSpeeds(
             tx,                 // X 不变
             targetTz,                 // Y 不变
@@ -341,6 +348,9 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
 
         return "Driving to AprilTag";
     }
+    public boolean isAlignedToAprilTag(){
+        return isAlignedToAprilTag;
+    }
     @Override
     public void periodic() {
 
@@ -352,7 +362,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,