climber

注意测试climber的起始调零功能，它将在enable后以-0.07的duty cycle out进行0.5秒运动，然后清零它的编码器。

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

@@ -0,0 +1,49 @@
+package frc.robot.controls;
+
+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.button.CommandXboxController;
+import frc.robot.subsystems.ClimberSubsystem;
+
+public class ClimberControls {
+
+    private final ClimberSubsystem climber;
+    private final CommandXboxController controller;
+
+    private final NetworkTable table;
+
+    public ClimberControls(ClimberSubsystem climber, CommandXboxController controller) {
+        this.climber = climber;
+        this.controller = controller;
+        this.table = NetworkTableInstance.getDefault().getTable("ClimberControls");
+    }
+
+    public Command defaultClimberCommand() {
+        return Commands.run(()->{
+            //climber.climbUp();
+                // 优先级：如果同时按，优先上升（你也可以反过来）
+                if (controller.x().getAsBoolean()) {
+                    climber.climbUp();
+                } else if (controller.y().getAsBoolean()) {
+                    climber.climbDown();
+                } else {
+                    // 松开：记录当前位置 + 锁死
+                    climber.captureHoldPosition();
+                    climber.holdPosition();
+                }
+
+                // 调试用
+                table.getEntry("xHeld_up").setBoolean(controller.x().getAsBoolean());
+                table.getEntry("yHeld_down").setBoolean(controller.y().getAsBoolean());
+        }, climber);
+
+    }
+
+    public Command enableInitCommand() {
+        return Commands.run(climber::initialize, climber)
+                .withTimeout(0.5)
+                .andThen(Commands.runOnce(climber::zeroEncoder, climber));
+    }
+}