1

src/main/deploy/pathplanner/autos/Shoot and Climb.auto

@@ -22,17 +22,53 @@
             "pathName": "Shoot to Climb Step1 Red"
           }
         },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 1.0
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "climb_up"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 1.0
+          }
+        },
         {
           "type": "path",
           "data": {
             "pathName": "Climb Step2 Red"
           }
         },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 1.0
+          }
+        },
         {
           "type": "path",
           "data": {
             "pathName": "Climb Step3 Red"
           }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 2.0
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "climb_down"
+          }
         }
       ]
     }

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

@@ -149,7 +149,7 @@ public final class Constants {
 
     public static double climbUpTicks = 94; // TODO: 实测后调整
     public static double climbDownTicks = 0; // TODO: 实测后调整
-    public static double climbDownAutoTicks = -5; // TODO: 实测后调整，自动爬时可能不需要完全降到底
+    public static double climbDownAutoTicks = 0; // TODO: 实测后调整，自动爬时可能不需要完全降到底
     public static final double climbVelocity = -200;
     public static final double holdToleranceRot = 0.5; 
     public static final double deltaDownTicks = 1;

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

@@ -190,7 +190,7 @@ public class RobotContainer {
   }
 
   private void configueClimber() {
-    enableTrigger.onTrue(climberControls.enableInitCommand());
+    //enableTrigger.onTrue(climberControls.enableInitCommand());
     climberSubsystem.setDefaultCommand(climberControls.defaultClimberCommand());
     //controllerupper.b().onTrue(climberControls.resetClimberEncoderCommand(climberSubsystem));
   }

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

@@ -49,17 +49,19 @@ import frc.robot.Constants.RobotStatus;
         public static Command climb_up(ClimberSubsystem climber) {
             return new RunCommand(() -> {
                 climber.climbUp();  // 控制爬升器上升
-            }, climber).withTimeout(1.0).andThen(new RunCommand(() -> {
+            }, climber).withTimeout(2).andThen(new RunCommand(() -> {
                 climber.climbUpNonStopforAuto();
             }, climber).withTimeout(0.5)).andThen(new InstantCommand(() -> {
+                climber.captureHoldPosition();  // 捕获当前爬升器位置以保持
                 climber.holdPosition();  // 停止并锁定当前爬升器位置
             }, climber));
         }
         public static Command climb_down(ClimberSubsystem climber) {
             return new RunCommand(() -> {
-                climber.climbDownAuto();  // 控制爬升器下降
+                climber.climbDown();  // 控制爬升器下降
             }, climber).withTimeout(2.0)  //
             .andThen(new InstantCommand(() -> {
+                climber.captureHoldPosition();  // 捕获当前爬升器位置以保持
                 climber.holdPosition();  // 停止并锁定当前爬升器位置
             }, climber));
         }

src/main/java/frc/robot/generated/TunerConstants.java

@@ -66,7 +66,17 @@ public class TunerConstants {
         );
     private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration();
     // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs
-    private static final Pigeon2Configuration pigeonConfigs = null;
+    private static final Pigeon2Configuration pigeonConfigs =
+    new Pigeon2Configuration()
+        .withGyroTrim(
+            new GyroTrimConfigs()
+                .withGyroScalarZ(-5.0)
+        )
+        .withPigeon2Features(
+            new Pigeon2FeaturesConfigs()
+                .withEnableCompass(false)
+        );
+
 
     // CAN bus that the devices are located on;
     // All swerve devices must share the same CAN bus