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

src/main/deploy/pathplanner/autos/Auto climb only.auto

@@ -0,0 +1,19 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "start_intake_timelimit"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Red Middle",
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/paths/Climb Step2 Red.path

@@ -8,20 +8,20 @@
       },
       "prevControl": null,
       "nextControl": {
-        "x": 15.610835339716738,
+        "x": 15.522355156858092,
-        "y": 3.3590426144037364
+        "y": 3.365
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 15.467,
+        "x": 15.517,
         "y": 3.365
       },
       "prevControl": {
-        "x": 15.743555432745389,
+        "x": 15.77785583456356,
-        "y": 3.3550226257587115
+        "y": 3.365
       },
       "nextControl": null,
       "isLocked": false,

src/main/deploy/pathplanner/paths/Climb Step3 Red.path

@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 15.467,
+        "x": 15.517,
         "y": 3.365
       },
       "prevControl": null,
       "nextControl": {
-        "x": 15.716589593303949,
+        "x": 15.767,
-        "y": 3.3506809597259686
+        "y": 3.365
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 15.467,
+        "x": 15.517,
         "y": 3.465
       },
       "prevControl": {
-        "x": 15.191301517038063,
+        "x": 15.239842716853756,
-        "y": 3.4933990509259307
+        "y": 3.465
       },
       "nextControl": null,
       "isLocked": false,

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

@@ -42,7 +42,7 @@ public final class Constants {
         public static final double backboardDownLimit = 0.0;  // 背板下限角度（根据实际需求设置）
     }
     public class AutoPositioning {
-        public static final double autoRotationkP = 0.1;
+        public static final double autoRotationkP = 0.08;
         public static final double autoRotationForBumpTargetDegrees = 45;
         public static final double autoRotationForTrenchTargetDegrees = 0;
         public static final boolean teleopOffsetEnabled = true;

src/main/java/frc/robot/Robot.java

@@ -3,6 +3,8 @@
 // the WPILib BSD license file in the root directory of this project.
 
 package frc.robot;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;

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

@@ -70,9 +70,19 @@ public class RobotContainer {
   private final SendableChooser<Command> autoChooser;
   
   private final Trigger enableTrigger = new Trigger(DriverStation::isEnabled);
+  private final Trigger teleopStartTrigger = new Trigger(DriverStation::isTeleopEnabled);
 
   
   public RobotContainer() {
+
+    Pose2d startingPose = Constants.StartingPoints.Red.Point2;
+    drivetrain.resetPose(startingPose);
+    configueSwerve();
+    configueShooter();
+    configueClimber();
+    bingdingLED();
+    configueIntaker();
+    //configureBindings();
     Autocommand.preNameCommands(
         climberSubsystem,
         IntakerSubsystem,
@@ -81,15 +91,9 @@ public class RobotContainer {
         driveControls,
         shooterControls
     );
-
     SignalLogger.enableAutoLogging(false);
     autoChooser = AutoBuilder.buildAutoChooser();
     SmartDashboard.putData("Auto Chooser", autoChooser);
-    configueSwerve();
-    configueShooter();
-    configueClimber();
-    bingdingLED();
-    configueIntaker();
   }
 
   /* ====================== */
@@ -200,4 +204,21 @@ public class RobotContainer {
     controllerupper.a().onTrue(intakerControls.resetIntakerRotaterEncoderCommand(IntakerSubsystem));
   }
 
+  // private void configureBindings() {
+  //       teleopStartTrigger.onTrue(handleClimberAtTeleopStartCommand());
+  //   }
+
+  // private Command handleClimberAtTeleopStartCommand() {
+  //       return new InstantCommand(() -> {
+  //           if (climberSubsystem.wasAutoClimbUsedInAuto()) {
+  //               climberSubsystem.climbUp();
+  //               climberSubsystem.clearAutoClimbUsedInAutoFlag();
+  //           }
+  //       }, climberSubsystem);
+  //   }
+
+  //   public ClimberSubsystem getClimberSubsystem() {
+  //       return climberSubsystem;
+  //   }
+
 }

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

@@ -32,7 +32,7 @@ import frc.robot.Constants.RobotStatus;
             ShooterControls shooterControls
     ) {
         NamedCommands.registerCommand("climb_up", climb_up(climber));
-        NamedCommands.registerCommand("climb_down", climb_down(climber));
+        //NamedCommands.registerCommand("climb_down", climb_down(climber));
         NamedCommands.registerCommand("start_intake", start_intake(intaker));
         NamedCommands.registerCommand("start_intake_timelimit", start_intake_timelimit(intaker));
         NamedCommands.registerCommand("stop_intake", stop_intake(intaker));
@@ -40,9 +40,9 @@ import frc.robot.Constants.RobotStatus;
         NamedCommands.registerCommand("intaker_up", intaker_up(intaker));
         NamedCommands.registerCommand("auto_align_to_climb", autoAlignToClimb(drivetrain));
         NamedCommands.registerCommand("shoot", shoot(shooter));
-        NamedCommands.registerCommand("Shake", Autocommand.shake(driveControls));
+        // NamedCommands.registerCommand("Shake", Autocommand.shake(driveControls));
-        NamedCommands.registerCommand("AutoAim", Autocommand.autoAim(driveControls));
+        // NamedCommands.registerCommand("AutoAim", Autocommand.autoAim(driveControls));
-        NamedCommands.registerCommand("AutoShoot", Autocommand.autoShoot(shooterControls));
+        // NamedCommands.registerCommand("AutoShoot", Autocommand.autoShoot(shooterControls));
 
     }
 
@@ -136,15 +136,15 @@ import frc.robot.Constants.RobotStatus;
                 shooter.setConveyorSpeedByRPS(0);  // 停止输送
             }, shooter)));
         }
-        public static Command autoAim(DriveControls driveControls) {
+        // public static Command autoAim(DriveControls driveControls) {
-            return driveControls.autoAimCommand();
+        //     return driveControls.autoAimCommand();
-        }
+        // }
-        public static Command autoShoot(ShooterControls shooterControl) {
+        // public static Command autoShoot(ShooterControls shooterControl) {
-            return shooterControl.autoShootToHubCommand();
+        //     return shooterControl.autoShootToHubCommand();
-        }
+        // }
-        public static Command shake(DriveControls driveControls) {
+        // public static Command shake(DriveControls driveControls) {
-            return driveControls.shakeCommand();
+        //     return driveControls.shakeCommand();
-        }
+        // }
 
 
     }

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

@@ -59,6 +59,4 @@ public class ClimberControls {
         return Commands.runOnce(climber::zeroEncoder, climber);
     }
     
-    
-    
 }

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

@@ -16,6 +16,8 @@ import frc.robot.Constants;
 
 public class ClimberSubsystem extends SubsystemBase {
 
+    // private boolean autoClimbUsedInAuto = false;
+
     private final TalonFX climberMotor;
     private double targetPositionTicks;
 
@@ -100,6 +102,18 @@ public class ClimberSubsystem extends SubsystemBase {
         climberMotor.set(0);
     }
 
+    // public void markAutoClimbUsedInAuto() {
+    //     autoClimbUsedInAuto = true;
+    // }
+
+    // public boolean wasAutoClimbUsedInAuto() {
+    //     return autoClimbUsedInAuto;
+    // }
+
+    // public void clearAutoClimbUsedInAutoFlag() {
+    //     autoClimbUsedInAuto = false;
+    // }
+
     @Override
     public void periodic() {
         climberTable.getEntry("velocityRps").setDouble(getVelocityRps());

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

@@ -163,7 +163,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
     
     public void forceUsingLimelightmt2() {
         LimelightHelpers.PoseEstimate mt2 =
-        LimelightHelpers.getBotPoseEstimate_wpiBlue(Constants.Limelight.LIMELIGHT_NAME_Shooter);
+        LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.Limelight.LIMELIGHT_NAME_Shooter);
 
         if (mt2 == null || mt2.tagCount == 0) {
             return;
@@ -258,7 +258,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
 
     private LimelightHelpers.PoseEstimate getLimelightPoseEstimate(String limelightName) {
         if (Constants.Limelight.UsingMetaTag2) {
-           return LimelightHelpers.getBotPoseEstimate_wpiBlue(limelightName);
+           return LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelightName);
         }
         return LimelightHelpers.getBotPoseEstimate_wpiBlue(limelightName);
     }
@@ -415,8 +415,8 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
         Pose2d pose = getState().Pose;
         m_field.setRobotPose(pose);
         posePub.set(pose);
-        shooter_ll_field.setRobotPose(LimelightHelpers.getBotPoseEstimate_wpiBlue(Constants.Limelight.LIMELIGHT_NAME_Shooter).pose);
+        shooter_ll_field.setRobotPose(LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.Limelight.LIMELIGHT_NAME_Shooter).pose);
-        intaker_ll_field.setRobotPose(LimelightHelpers.getBotPoseEstimate_wpiBlue(Constants.Limelight.LIMELIGHT_NAME_Intaker).pose);
+        intaker_ll_field.setRobotPose(LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.Limelight.LIMELIGHT_NAME_Intaker).pose);
         driveNetworkTable.getEntry("YawFromState").setDouble(getState().Pose.getRotation().getDegrees());
         driveNetworkTable.getEntry("DriveMode").setString(m_driveMode.name());
         driveNetworkTable.getEntry("IsFieldCentric").setBoolean(m_driveMode == DriveMode.FIELD_CENTRIC);