climb led

SaintsRobotics · Mar 30, 2024 · 2713cd9 · 2713cd9
1 parent 34e7d4d
commit 2713cd9
Show file tree

Hide file tree

Showing 3 changed files with 23 additions and 2 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -221,7 +221,8 @@ private void configureBindings() {
 
     // Amp Outtake, Operator Controller X Button
     new JoystickButton(m_operatorController, Button.kX.value)
-        .onTrue(new AmpOuttakeCommand(m_intakeSubsystem));
+        .onTrue(new AmpOuttakeCommand(m_intakeSubsystem))
+        .onFalse(new InstantCommand(() -> m_intakeSubsystem.stopIntake()));
 
     // Spin up Shooter, Operator Controller Left Trigger
     new Trigger(() -> {

diff --git a/src/main/java/frc/robot/commands/DefaultLEDCommand.java b/src/main/java/frc/robot/commands/DefaultLEDCommand.java
@@ -64,7 +64,7 @@ public void initialize() {
     }
 
     //check for pneumatics state and override b4 value
-    if (m_climberSubsystem.getState() == Value.kForward){
+    if (m_climberSubsystem.getState() == Value.kForward || m_climberSubsystem.getDeployed()){
       rgb[0] = 257;
       rgb[1] = 257;
       rgb[2] = 257;

diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
@@ -8,6 +8,7 @@
 import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
 import edu.wpi.first.wpilibj.PneumaticHub;
 import edu.wpi.first.wpilibj.PneumaticsModuleType;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.ClimberConstants;
@@ -19,6 +20,9 @@ public class ClimberSubsystem extends SubsystemBase {
       ClimberConstants.rightReverseChannel, ClimberConstants.rightForwardChannel);
   private PneumaticHub m_pHub;
 
+  private boolean deployed = false;
+  private Timer m_timer = new Timer();
+
   private boolean m_compressorEnabled;
 
   private Value m_state;
@@ -29,6 +33,8 @@ public ClimberSubsystem() {
     solenoidOff();
 
     m_compressorEnabled = false;
+
+    m_timer.start();
   }
 
   // Runs once every tick (~20ms)
@@ -39,6 +45,16 @@ public void periodic() {
     SmartDashboard.putNumber("pressure", m_pHub.getPressure(0));
     SmartDashboard.putBoolean("Compressor Enabled", m_compressorEnabled);
     SmartDashboard.putBoolean("Compressor Running", m_pHub.getCompressor());
+
+    if (m_state == Value.kForward){
+      deployed = true;
+    }
+    if (m_state == Value.kReverse){
+      m_timer.reset();
+      if (m_timer.get() > 3.0){
+        deployed = false;
+      }
+    }
   }
 
   /**
@@ -66,6 +82,10 @@ public Value getState(){
     return m_state;
   }
 
+  public boolean getDeployed(){
+    return deployed;
+  }
+
   /**
    * Toggles the state of the compressor (on/off)
    */