diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 481626e..106382e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -103,7 +103,8 @@ public static final class IntakeConstants { // In degrees public static final double kIntakeLoweredAngle = -193; public static final double kIntakeRaisedAngle = 0; - public static final double kIntakeAmpScoringAngle = -71; // 193 - 100 (previous angle) + public static final double kIntakeAmpScoringAngle = -71; + public static final int kIntakeAmpTolerance = 2; /** Encoder offset in rotations */ public static final double kArmEncoderOffset = 0.715; diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 55939bc..9d35fd1 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -81,7 +81,7 @@ public double getArmPosition() { } public boolean ampReady(){ - return m_armEncoder.getDistance() < IntakeConstants.kIntakeAmpScoringAngle + 2 && m_armEncoder.getDistance() > IntakeConstants.kIntakeAmpScoringAngle - 2; + return m_armEncoder.getDistance() < IntakeConstants.kIntakeAmpScoringAngle + IntakeConstants.kIntakeAmpTolerance && m_armEncoder.getDistance() > IntakeConstants.kIntakeAmpScoringAngle - IntakeConstants.kIntakeAmpTolerance; } public boolean armAtSetpoint() {