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

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

@@ -109,5 +109,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.Constants.RobotStatus;
 import frc.robot.controls.DriveControls;
 import frc.robot.controls.ShooterControls;
@@ -21,7 +23,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 {
 
   /* ====================== */
@@ -37,6 +40,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 子系统
@@ -56,6 +60,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() {
@@ -64,6 +70,7 @@ public class RobotContainer {
     SmartDashboard.putData("Auto Chooser", autoChooser);
     configueSwerve();
     configueShooter();
+    configueClimber();
     bingdingLED();
   }
 
@@ -146,4 +153,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);
+    }
+}