From 590100af5969ab6e3546691415b0a3bcf28f68a9 Mon Sep 17 00:00:00 2001
From: ProgrammingSR <programming@saintsrobotics.com>
Date: Fri, 22 Mar 2024 19:41:32 -0700
Subject: [PATCH] Final changes

---
 src/main/java/frc/robot/Constants.java                 |  4 ++--
 src/main/java/frc/robot/RobotContainer.java            |  6 +++---
 .../java/frc/robot/subsystems/ClimberSubsystem.java    |  2 ++
 .../java/frc/robot/subsystems/IntakeSubsystem.java     | 10 +++++-----
 .../java/frc/robot/subsystems/ShooterSubsystem.java    |  4 ++--
 5 files changed, 14 insertions(+), 12 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 8dfc7e7..39a2bdb 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -110,8 +110,8 @@ public static final class IntakeConstants {
 
     public static final double kIntakeSpeed = 0.5;
 
-    // public static final int kProximityThreshold = 130;
-    public static final int kIRThreshold = 2;
+    public static final int kProximityThreshold = 130;
+    // public static final int kIRThreshold = 2;
   }
 
   public static final class ShooterConstants {
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 3bdb256..0c22866 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -227,14 +227,14 @@ private void configureBindings() {
       return m_operatorController.getBButton() && m_operatorController.getRightBumper();
     }).whileTrue(new InstantCommand(() -> m_climberSubsystem.reverse()));
 
-    // Toggle Distance Sensor, Operator Controller Left Bumper + Start Button
+    // Toggle Color Sensor, Operator Controller Left Bumper + Start Button
     new Trigger(() -> {
       return m_operatorController.getLeftBumper() && m_operatorController.getStartButton();
     }).onTrue(new InstantCommand(() -> m_intakeSubsystem.colorSensorToggle()));
 
-    // Toggle Compressor, Operator Controller Right Bumper + Menu
+    // Toggle Compressor, Operator Controller Right Bumper + Back Button
     new Trigger(() -> {
-      return m_operatorController.getRightBumper() && m_operatorController.getStartButton();
+      return m_operatorController.getLeftBumper() && m_operatorController.getBackButton();
     }).onTrue(new InstantCommand(() -> m_climberSubsystem.toggleCompressor()));
   }
 
diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
index 7a2f791..3a42743 100644
--- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
@@ -37,6 +37,8 @@ public void periodic() {
     m_rightSolenoid.set(m_state);
     SmartDashboard.putString("pneumatics state", m_state.name());
     SmartDashboard.putNumber("pressure", m_pHub.getPressure(0));
+    SmartDashboard.putBoolean("Compressor Enabled", m_compressorEnabled);
+    SmartDashboard.putBoolean("Compressor Running", m_pHub.getCompressor());
   }
 
   /**
diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
index 237adf2..20c50b8 100644
--- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
@@ -38,7 +38,6 @@ public class IntakeSubsystem extends SubsystemBase {
   /** Creates a new IntakeSubsystem */
   public IntakeSubsystem() {
     m_armEncoder.setPositionOffset(IntakeConstants.kArmEncoderOffset);
-    SmartDashboard.putNumber("arm", m_armEncoder.getAbsolutePosition());
     m_armEncoder.setDistancePerRotation(360);
 
     m_intakeMotor.setIdleMode(IdleMode.kCoast);
@@ -97,13 +96,13 @@ public void stopIntake() {
   /**
    * Gets distance from Color sensor
    */
-  public int getColorIR() {
-    return m_colorSensor.getIR();
+  public int getColorProximity() {
+    return m_colorSensor.getProximity();
   }
 
   @Override
   public void periodic() {
-    haveNote = getColorIR() > IntakeConstants.kIRThreshold;
+    haveNote = m_colorSensorToggle ? getColorProximity() > IntakeConstants.kProximityThreshold : false;
 
     double armMotorSpeed = MathUtil.clamp(m_armPID.calculate(m_armEncoder.getDistance(), m_armSetpoint), -0.3, 0.3);
     m_armMotor.set(armMotorSpeed);
@@ -113,7 +112,8 @@ public void periodic() {
     SmartDashboard.putNumber("Arm Absolute Angle", m_armEncoder.getAbsolutePosition());
     SmartDashboard.putBoolean("Have Note?", haveNote);
     // SmartDashboard.putNumber("pid output", armMotorSpeed);
-    SmartDashboard.putNumber("Proximity", getColorIR());
+    SmartDashboard.putNumber("Proximity", m_colorSensor.getProximity());
+    SmartDashboard.putBoolean("Color Sensor Toggle", m_colorSensorToggle);
     SmartDashboard.putNumber("IR", m_colorSensor.getIR());
   }
 
diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
index f54afd1..55c14ea 100644
--- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
@@ -60,8 +60,8 @@ public double returnCurrentSpeed() {
   @Override
   public void periodic() {
     // This method will be called once per scheduler run
-    SmartDashboard.putNumber("bottom Speed", m_bottomSpeed);
-    SmartDashboard.putNumber("top Speed", m_topSpeed);
+    // SmartDashboard.putNumber("bottom Speed", m_bottomSpeed);
+    // SmartDashboard.putNumber("top Speed", m_topSpeed);
 
     m_bottom.set(m_bottomSpeed);
     m_top.set(m_topSpeed);