diff --git a/src/main/java/frc/robot/intake/IntakeSubsystem.java b/src/main/java/frc/robot/intake/IntakeSubsystem.java index 9fdddc9..54fbde1 100644 --- a/src/main/java/frc/robot/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/intake/IntakeSubsystem.java @@ -26,6 +26,7 @@ public IntakeSubsystem(CANSparkMax motor) { super(SubsystemPriority.INTAKE); this.motor = motor; encoder = motor.getEncoder(); + encoder.setPositionConversionFactor(1.0); encoder.setVelocityConversionFactor(1.0); } @@ -87,10 +88,10 @@ public void enabledPeriodic() { gamePiece = HeldGamePiece.CONE; } else if (motorVelocity < threshold && goalState == IntakeState.INTAKE_CUBE) { gamePiece = HeldGamePiece.CUBE; + } else if (motorVelocity > threshold + && (goalState == IntakeState.OUTTAKE_CONE || goalState == IntakeState.OUTTAKE_CUBE)) { + gamePiece = HeldGamePiece.NOTHING; } - } else if (motorVelocity > threshold - && (goalState == IntakeState.OUTTAKE_CONE || goalState == IntakeState.OUTTAKE_CUBE)) { - gamePiece = HeldGamePiece.NOTHING; } }