climber

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

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

@@ -96,5 +96,28 @@ public final class Constants {
     public static final double positionToleranceRotations = 0.00;
     //rotater容差
     }   
-    
+    public static class Climber {
+
+    public static final int motorID = 49;
+
+    public static final boolean inverted = false;
+
+    public static final double climbUpTicks = 83; // TODO: 实测后调整
+    public static final double climbDownTicks = 0; // TODO: 实测后调整
+    public static final double holdToleranceRot = 0.5; 
+
+    public static final Slot0Configs slot0Configs = new Slot0Configs();
+
+    static {
+        // TODO: PID/F 值你们实测后填
+        slot0Configs.kP = 0.2;
+        slot0Configs.kI = 0.0;
+        slot0Configs.kD = 0.0;
+
+        // TODO: 如果要前馈（速度闭环一般建议有 kV）
+        slot0Configs.kS = 0.0;
+        slot0Configs.kV = 0.1;
+        slot0Configs.kA = 0.0;
+    }
+}
 }

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

@@ -4,12 +4,14 @@ import com.ctre.phoenix6.SignalLogger;
 import com.pathplanner.lib.auto.AutoBuilder;
 
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj.DriverStation;
 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.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.controls.DriveControls;
 import frc.robot.controls.ShooterControls;
 import frc.robot.generated.TunerConstants;
@@ -20,7 +22,8 @@ import frc.robot.tuning.ConfigTalonFXMotorTuner;
 import frc.robot.tuning.ConfigTalonFXSMotorTuner;
 import frc.robot.tuning.DriveGainsTuner;
 import frc.robot.utils.LedBindings;
-
+import frc.robot.controls.ClimberControls;
+import frc.robot.subsystems.ClimberSubsystem;
 public class RobotContainer {
 
   /* ====================== */
@@ -36,6 +39,7 @@ public class RobotContainer {
 
   // Phoenix TunerX 生成的底盘
   public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
+  public final ClimberSubsystem climberSubsystem = new ClimberSubsystem();
   public final ShooterSubsystem shooterSubsystem = new ShooterSubsystem();
 
   // LED 子系统
@@ -46,6 +50,7 @@ public class RobotContainer {
   /* ====================== */
 
   private final DriveControls driveControls = new DriveControls(drivetrain, controller);
+  private final ClimberControls climberControls = new ClimberControls(climberSubsystem, controller);
   private final ShooterControls shooterControls = new ShooterControls(shooterSubsystem, controller);
 
   private final DriveGainsTuner driveGainsTuner = new DriveGainsTuner(drivetrain);
@@ -54,6 +59,8 @@ public class RobotContainer {
   private ConfigTalonFXSMotorTuner configBackboardTuner = new ConfigTalonFXSMotorTuner(shooterSubsystem.backboardMotor, "backboard", Constants.Shooter.backboardSlot0Configs);
 
   private final SendableChooser<Command> autoChooser;
+  
+  private final Trigger enableTrigger = new Trigger(DriverStation::isEnabled);
 
   public RobotContainer() {
     SignalLogger.enableAutoLogging(false);
@@ -61,6 +68,7 @@ public class RobotContainer {
     SmartDashboard.putData("Auto Chooser", autoChooser);
     configueSwerve();
     configueShooter();
+    configueClimber();
     bingdingLED();
   }
 
@@ -131,4 +139,11 @@ public class RobotContainer {
     shooterSubsystem.setRightFollowLeft();
   }
 
+  private void configueClimber() {
+    
+    enableTrigger.onTrue(climberControls.enableInitCommand());
+
+    climberSubsystem.setDefaultCommand(climberControls.defaultClimberCommand());
+  }
+
 }

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

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

@@ -0,0 +1,96 @@
+package frc.robot.subsystems;
+
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.controls.DutyCycleOut;
+import com.ctre.phoenix6.controls.PositionVoltage;
+import com.ctre.phoenix6.controls.VelocityDutyCycle;
+import com.ctre.phoenix6.controls.VelocityVoltage;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
+
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+
+
+public class ClimberSubsystem extends SubsystemBase {
+
+    private final TalonFX climberMotor;
+    private double targetPositionTicks;
+
+    private final PositionVoltage positionHoldRequest =
+            new PositionVoltage(0).withSlot(0);
+
+    private final VelocityVoltage velocityRequest = new VelocityVoltage(0).withSlot(0);
+
+    private double holdPositionTicks = 0.0;
+
+    private final NetworkTable climberTable;
+    
+
+    public ClimberSubsystem() {
+        climberMotor = new TalonFX(Constants.Climber.motorID);
+
+        climberMotor.getConfigurator().apply(Constants.Climber.slot0Configs);
+
+        var cfg = new MotorOutputConfigs();
+        cfg.Inverted = Constants.Climber.inverted
+                ? InvertedValue.Clockwise_Positive
+                : InvertedValue.CounterClockwise_Positive;
+        climberMotor.getConfigurator().apply(cfg);
+
+        climberTable = NetworkTableInstance.getDefault().getTable("Climber");
+
+        holdPositionTicks = getPositionTicks();
+    }
+    public void initialize() {
+        
+        climberMotor.set(-0.07);
+
+    }
+    public void zeroEncoder(){
+
+        climberMotor.setPosition(0);
+        stopMotor();
+    }
+
+    public double getPositionTicks() {
+        return climberMotor.getPosition().getValueAsDouble();
+    }
+
+    public double getVelocityRps() {
+        return climberMotor.getVelocity().getValueAsDouble();
+    }
+
+    public void climbUp() {
+        targetPositionTicks = Constants.Climber.climbUpTicks;  
+        climberMotor.setControl(positionHoldRequest.withPosition(targetPositionTicks));
+    }
+
+    public void climbDown() {
+        targetPositionTicks = Constants.Climber.climbDownTicks;  
+        climberMotor.setControl(positionHoldRequest.withPosition(targetPositionTicks));
+    }
+
+    public void captureHoldPosition() {
+        holdPositionTicks = getPositionTicks();
+    }
+
+    public void holdPosition() {
+        climberMotor.setControl(
+                positionHoldRequest.withPosition(holdPositionTicks)
+        );
+    }
+
+    public void stopMotor() {
+        climberMotor.set(0);
+    }
+
+    @Override
+    public void periodic() {
+        climberTable.getEntry("velocityRps").setDouble(getVelocityRps());
+        climberTable.getEntry("positionTicks").setDouble(getPositionTicks());
+        climberTable.getEntry("holdPositionTicks").setDouble(holdPositionTicks);
+    }
+}