Merge branch 'amp' of https://github.com/SaintsRobotics/Crescendo2024 …

…into amp
SaintsRobotics · Mar 30, 2024 · 8788c5f · 8788c5f
2 parents 1332789 + 0fd6465
commit 8788c5f
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 1 deletion.
diff --git a/src/main/java/frc/robot/commands/DefaultLEDCommand.java b/src/main/java/frc/robot/commands/DefaultLEDCommand.java
@@ -71,7 +71,7 @@ public void initialize() {
     }
 
     if (m_intakeSubsystem.getArmPosition() == IntakeConstants.kIntakeAmpScoringAngle){
-      if (m_intakeSubsystem.armAtSetpoint()){
+      if (m_intakeSubsystem.ampReady()){
         rgb[0] = 0;
         rgb[1] = 255;
         rgb[2] = 0;   

diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
@@ -80,6 +80,10 @@ public double getArmPosition() {
     return m_armSetpoint;
   }
 
+  public boolean ampReady(){
+    return ((m_armEncoder.getDistance() < IntakeConstants.kIntakeAmpScoringAngle + 1) || (m_armEncoder.getDistance() > IntakeConstants.kIntakeAmpScoringAngle - 1)); 
+  }
+
   public boolean armAtSetpoint() {
     return m_armPID.atSetpoint();
   }