climber (#20)

* chore: fixed logic and refactored * feat: added user button functionality
SaintsRobotics · Feb 27, 2024 · e526eec · e526eec
1 parent fce6055
commit e526eec
Show file tree

Hide file tree

Showing 3 changed files with 66 additions and 57 deletions.
diff --git 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
@@ -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
@@ -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();
     }
+  }
 }