From 0fe28c265362804dc550eafcd139580538cdba83 Mon Sep 17 00:00:00 2001 From: Alyssum-chenyiyi <13162501939@163.com> Date: Tue, 24 Feb 2026 14:43:28 +0800 Subject: [PATCH] climber MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 注意测试climber的起始调零功能，它将在enable后以-0.07的duty cycle out进行0.5秒运动，然后清零它的编码器。 --- src/main/java/frc/robot/Constants.java | 25 ++++- src/main/java/frc/robot/RobotContainer.java | 17 +++- .../frc/robot/controls/ClimberControls.java | 49 ++++++++++ .../robot/subsystems/ClimberSubsystem.java | 96 +++++++++++++++++++ 4 files changed, 185 insertions(+), 2 deletions(-) create mode 100644 src/main/java/frc/robot/controls/ClimberControls.java create mode 100644 src/main/java/frc/robot/subsystems/ClimberSubsystem.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3dfc4f3..f032b60 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/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; + } +} } \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 71eac46..895d0f1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/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 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()); + } + } diff --git a/src/main/java/frc/robot/controls/ClimberControls.java b/src/main/java/frc/robot/controls/ClimberControls.java new file mode 100644 index 0000000..119b172 --- /dev/null +++ b/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)); + } +} diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java new file mode 100644 index 0000000..20eccac --- /dev/null +++ b/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); + } +}