climber

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

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