diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 687a0a0..882ff45 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,9 +4,13 @@ package frc.robot; + +import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.subsystems.ClimberSubsystem; /** * The VM is configured to automatically run this class, and to call the functions corresponding to @@ -19,6 +23,9 @@ public class Robot extends TimedRobot { private RobotContainer m_robotContainer; + public ClimberSubsystem m_climberSubsystem = new ClimberSubsystem(); + + private Timer m_buttonTimer = new Timer(); /** * This function is run when the robot is first started up and should be used for any * initialization code. @@ -51,7 +58,12 @@ public void robotPeriodic() { public void disabledInit() {} @Override - public void disabledPeriodic() {} + public void disabledPeriodic() { + if (RobotController.getUserButton() && m_buttonTimer.get() > 1) { + m_climberSubsystem.toggleCompressor(); + m_buttonTimer.reset(); + } + } /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e775cff..e0da380 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -113,8 +113,10 @@ private void configureBindings() { .onTrue(new InstantCommand(() -> m_robotDrive.zeroHeading(), m_robotDrive)); // new JoystickButton(m_driverController, Button.kA.value).whileTrue( - // AutoBuilder.pathfindToPose(new Pose2d(2.8, 5.5, new Rotation2d()), new PathConstraints( - // DriveConstants.kMaxSpeedMetersPerSecond - 1, 5, DriveConstants.kMaxAngularSpeedRadiansPerSecond - 1, 5))); + // AutoBuilder.pathfindToPose(new Pose2d(2.8, 5.5, new Rotation2d()), new + // PathConstraints( + // DriveConstants.kMaxSpeedMetersPerSecond - 1, 5, + // DriveConstants.kMaxAngularSpeedRadiansPerSecond - 1, 5))); new JoystickButton(m_operatorController, Button.kX.value) .onTrue(new InstantCommand(() -> m_shooterSubsystem.spin(0.75), m_shooterSubsystem)) @@ -124,9 +126,9 @@ private void configureBindings() { .onFalse(new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem)); new JoystickButton(m_operatorController, Button.kA.value) - .onTrue(new InstantCommand(() -> m_climberSubsystem.forward(), m_robotDrive)); + .onTrue(new InstantCommand(() -> m_climberSubsystem.forward(), m_climberSubsystem)); new JoystickButton(m_operatorController, Button.kB.value) - .onTrue(new InstantCommand(() -> m_climberSubsystem.reverse(), m_robotDrive)); + .onTrue(new InstantCommand(() -> m_climberSubsystem.reverse(), m_climberSubsystem)); new Trigger(() -> { return m_driverController.getLeftTriggerAxis() > 0.5; diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java index dbb7830..1072b4b 100644 --- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java @@ -3,74 +3,69 @@ import edu.wpi.first.wpilibj.Compressor; import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj.PneumaticsModuleType; - +import edu.wpi.first.wpilibj.DoubleSolenoid.Value; +import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.ClimberConstants; import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kForward; import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kOff; import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kReverse; -public class ClimberSubsystem { +public class ClimberSubsystem extends SubsystemBase{ - private final DoubleSolenoid m_leftSolenoid = new DoubleSolenoid(PneumaticsModuleType.REVPH, - ClimberConstants.leftForwardChannel, ClimberConstants.leftReverseChannel); - private final DoubleSolenoid m_rightSolenoid = new DoubleSolenoid(PneumaticsModuleType.REVPH, - ClimberConstants.rightForwardChannel, ClimberConstants.rightReverseChannel); + private final DoubleSolenoid m_leftSolenoid = new DoubleSolenoid(PneumaticsModuleType.REVPH, + ClimberConstants.leftForwardChannel, ClimberConstants.leftReverseChannel); + private final DoubleSolenoid m_rightSolenoid = new DoubleSolenoid(PneumaticsModuleType.REVPH, + ClimberConstants.rightForwardChannel, ClimberConstants.rightReverseChannel); - private final Compressor m_compressor = new Compressor(PneumaticsModuleType.REVPH); + private final Compressor m_compressor = new Compressor(PneumaticsModuleType.REVPH); - private boolean enableCompressor = true; + private boolean m_compressorEnabled; - public ClimberSubsystem() { - m_leftSolenoid.set(kOff); - m_rightSolenoid.set(kOff); - m_compressor.disable(); - m_compressor.enableAnalog(ClimberConstants.minPressure, ClimberConstants.maxPressure); - } + private Value m_state; - // Runs once every tick (~20ms) - public void periodic() { + public ClimberSubsystem() { + m_compressorEnabled = false; + solenoidOff(); + toggleCompressor(); } - /** - * Sets the state of the solenoid to off - */ - public void off() { - m_leftSolenoid.set(kOff); - m_rightSolenoid.set(kOff); - } + // Runs once every tick (~20ms) + public void periodic() { + m_leftSolenoid.set(m_state); + m_rightSolenoid.set(m_state); + } - /** - * Extends both arms - */ - public void forward() { - m_leftSolenoid.set(kForward); - m_rightSolenoid.set(kForward); - } + /** + * Sets the state of the solenoid to off + */ + public void solenoidOff() { + m_state = kOff; + } - /** - * Retracts both arms - */ - public void reverse() { - m_leftSolenoid.set(kReverse); - m_rightSolenoid.set(kReverse); - } - /* - * Toggles the state of the climber - */ + /** + * Extends both arms + */ + public void forward() { + m_state = kForward; + } - public void toggle() { - m_leftSolenoid.toggle(); - m_rightSolenoid.toggle(); - } + /** + * Retracts both arms + */ + public void reverse() { + m_state = kReverse; + } - // Toggles the state of the compressor (on/off) - public void toggleCompresor() { - enableCompressor = !enableCompressor; - if (enableCompressor) { - m_compressor.enableAnalog(ClimberConstants.minPressure, ClimberConstants.maxPressure); - } else { - m_compressor.disable(); - } + /** + * Toggles the state of the compressor (on/off) + */ + public void toggleCompressor() { + m_compressorEnabled = !m_compressorEnabled; + if (m_compressorEnabled) { + m_compressor.enableAnalog(ClimberConstants.minPressure, ClimberConstants.maxPressure); + } else { + m_compressor.disable(); } + } }