diff --git a/src/main/java/frc/robot/queuer/QueuerSubsystem.java b/src/main/java/frc/robot/queuer/QueuerSubsystem.java index 39ff32e5..8c0be32f 100644 --- a/src/main/java/frc/robot/queuer/QueuerSubsystem.java +++ b/src/main/java/frc/robot/queuer/QueuerSubsystem.java @@ -14,13 +14,11 @@ import org.littletonrobotics.junction.Logger; public class QueuerSubsystem extends LifecycleSubsystem { - private static final double NOTE_SHUFFLE_DURATION = 0.5; private final TalonFX motor; private final DigitalInput sensor; private QueuerState goalState = QueuerState.IDLE; private final Debouncer debouncer = RobotConfig.get().queuer().debouncer(); private boolean debouncedSensor = false; - private final Timer shuffleTimer = new Timer(); public QueuerSubsystem(TalonFX motor, DigitalInput sensor) { super(SubsystemPriority.QUEUER); @@ -48,16 +46,8 @@ public void enabledPeriodic() { break; case SHUFFLE: if (sensorHasNote()) { - if (shuffleTimer.hasElapsed(NOTE_SHUFFLE_DURATION)) { - if (shuffleTimer.hasElapsed(NOTE_SHUFFLE_DURATION * 2)) { - // Allow note to expand - motor.disable(); - shuffleTimer.reset(); - } else { - // Push note towards intake - motor.setVoltage(-2); - } - } + // We have the note, try pushing it towards intake to shuffle it around + motor.setVoltage(-1); } else { // Note is out of sensor range, try intaking it motor.setVoltage(1);