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 a0a33dd..a38bf22 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 + 1) || (m_armEncoder.getDistance() > IntakeConstants.kIntakeAmpScoringAngle - 1)); + return m_armEncoder.getDistance() < (IntakeConstants.kIntakeAmpScoringAngle + IntakeConstants.kIntakeAmpTolerance) && m_armEncoder.getDistance() > (IntakeConstants.kIntakeAmpScoringAngle - IntakeConstants.kIntakeAmpTolerance); } public boolean armAtSetpoint() {