From 36359cd123678fbc2334a4a8db62005b2aa1d597 Mon Sep 17 00:00:00 2001
From: SR1899 <programming@saintsrobotics.com>
Date: Sat, 30 Mar 2024 11:55:01 -0700
Subject: [PATCH] keep it rainbow while climbing

---
 .../java/frc/robot/commands/DefaultLEDCommand.java |  2 +-
 .../frc/robot/subsystems/ClimberSubsystem.java     | 14 ++++++++++++++
 2 files changed, 15 insertions(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/commands/DefaultLEDCommand.java b/src/main/java/frc/robot/commands/DefaultLEDCommand.java
index b598a8f..58b166c 100644
--- 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.deployed()){
       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
index 9bd6f9c..f3df0c2 100644
--- 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;
@@ -23,6 +24,9 @@ public class ClimberSubsystem extends SubsystemBase {
 
   private Value m_state;
 
+  private double togglecount = 0;
+  private Timer m_timer = new Timer();
+
   public ClimberSubsystem() {
     m_pHub = new PneumaticHub(2);
 
@@ -39,6 +43,10 @@ public void periodic() {
     SmartDashboard.putNumber("pressure", m_pHub.getPressure(0));
     SmartDashboard.putBoolean("Compressor Enabled", m_compressorEnabled);
     SmartDashboard.putBoolean("Compressor Running", m_pHub.getCompressor());
+
+    if (togglecount >= 3){
+      togglecount = 0;
+    }
   }
 
   /**
@@ -53,6 +61,7 @@ public void solenoidOff() {
    */
   public void forward() {
     m_state = kForward;
+    togglecount++;
   }
 
   /**
@@ -60,12 +69,17 @@ public void forward() {
    */
   public void reverse() {
     m_state = kReverse;
+    togglecount++;
   }
 
   public Value getState(){
     return m_state;
   }
 
+  public boolean deployed(){
+    return togglecount > 0;
+  }
+
   /**
    * Toggles the state of the compressor (on/off)
    */