diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2f94c93..4fda7db 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -70,6 +70,7 @@ public class RobotContainer { private final SendableChooser autoChooser; private final Trigger enableTrigger = new Trigger(DriverStation::isEnabled); + private final Trigger teleopStartTrigger = new Trigger(DriverStation::isTeleopEnabled); public RobotContainer() { @@ -90,6 +91,7 @@ public class RobotContainer { configueClimber(); bingdingLED(); configueIntaker(); + configureBindings(); } /* ====================== */ @@ -200,4 +202,21 @@ public class RobotContainer { controllerupper.a().onTrue(intakerControls.resetIntakerRotaterEncoderCommand(IntakerSubsystem)); } + private void configureBindings() { + teleopStartTrigger.onTrue(handleClimberAtTeleopStartCommand()); + } + + private Command handleClimberAtTeleopStartCommand() { + return new InstantCommand(() -> { + if (climberSubsystem.wasAutoClimbUsedInAuto()) { + climberSubsystem.climbUp(); + climberSubsystem.clearAutoClimbUsedInAutoFlag(); + } + }, climberSubsystem); + } + + public ClimberSubsystem getClimberSubsystem() { + return climberSubsystem; + } + } diff --git a/src/main/java/frc/robot/controls/Autocommand.java b/src/main/java/frc/robot/controls/Autocommand.java index 46f83c8..dd2de02 100644 --- a/src/main/java/frc/robot/controls/Autocommand.java +++ b/src/main/java/frc/robot/controls/Autocommand.java @@ -56,13 +56,17 @@ import frc.robot.Constants.RobotStatus; }, climber)); } public static Command climb_down(ClimberSubsystem climber) { - return new RunCommand(() -> { - climber.climbDownAuto(); // 控制爬升器下降 - }, climber).withTimeout(2.0) // - .andThen(new InstantCommand(() -> { - climber.holdPosition(); // 停止并锁定当前爬升器位置 - }, climber)); + return Commands.sequence( + new InstantCommand(climber::markAutoClimbUsedInAuto), + new RunCommand(() -> { + climber.climbDownAuto(); + }, climber).withTimeout(2.0), + new InstantCommand(() -> { + climber.holdPosition(); + }, climber) + ); } + public static Command start_intake(IntakerSubsystem intaker) { return new RunCommand(() -> { intaker.setIntakerGetterSpeed(Constants.Intaker.IntakeGetterSpeedforAuto); // 启动吸球 diff --git a/src/main/java/frc/robot/controls/ClimberControls.java b/src/main/java/frc/robot/controls/ClimberControls.java index 570d323..2d3f6aa 100644 --- a/src/main/java/frc/robot/controls/ClimberControls.java +++ b/src/main/java/frc/robot/controls/ClimberControls.java @@ -58,7 +58,5 @@ public class ClimberControls { public Command resetClimberEncoderCommand(ClimberSubsystem climber) { return 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 index 669a15a..e01dd1a 100644 --- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java @@ -16,6 +16,8 @@ import frc.robot.Constants; public class ClimberSubsystem extends SubsystemBase { + private boolean autoClimbUsedInAuto = false; + private final TalonFX climberMotor; private double targetPositionTicks; @@ -100,6 +102,18 @@ public class ClimberSubsystem extends SubsystemBase { climberMotor.set(0); } + public void markAutoClimbUsedInAuto() { + autoClimbUsedInAuto = true; + } + + public boolean wasAutoClimbUsedInAuto() { + return autoClimbUsedInAuto; + } + + public void clearAutoClimbUsedInAutoFlag() { + autoClimbUsedInAuto = false; + } + @Override public void periodic() { climberTable.getEntry("velocityRps").setDouble(getVelocityRps());