diff --git a/src/main/java/frc/robot/commands/DefaultLEDCommand.java b/src/main/java/frc/robot/commands/DefaultLEDCommand.java index 4e62564..727b663 100644 --- 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 index d1adf3f..a69ffef 100644 --- 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(); }