diff --git a/src/main/java/frc/robot/conveyor/ConveyorState.java b/src/main/java/frc/robot/conveyor/ConveyorState.java index a4259617..e872b69d 100644 --- a/src/main/java/frc/robot/conveyor/ConveyorState.java +++ b/src/main/java/frc/robot/conveyor/ConveyorState.java @@ -12,6 +12,7 @@ public enum ConveyorState { QUEUER_TO_INTAKE, WAITING_AMP_SHOT, SHUFFLE, + SHUFFLE_ASSIST_WHEN_QUEUER_SENSOR_TURNS_OFF, AMP_SHOT, /** Turns on at 12V and then off every n seconds. */ TRAP_SHOT_PULSE; diff --git a/src/main/java/frc/robot/conveyor/ConveyorSubsystem.java b/src/main/java/frc/robot/conveyor/ConveyorSubsystem.java index 41376ea3..6c77b897 100644 --- a/src/main/java/frc/robot/conveyor/ConveyorSubsystem.java +++ b/src/main/java/frc/robot/conveyor/ConveyorSubsystem.java @@ -61,6 +61,9 @@ public void robotPeriodic() { case INTAKE_TO_QUEUER: motor.setVoltage(3); break; + case SHUFFLE_ASSIST_WHEN_QUEUER_SENSOR_TURNS_OFF: + motor.setVoltage(4); + break; case QUEUER_TO_INTAKE: motor.setVoltage(-12); break; diff --git a/src/main/java/frc/robot/intake/IntakeState.java b/src/main/java/frc/robot/intake/IntakeState.java index 52fd6dc9..46047e83 100644 --- a/src/main/java/frc/robot/intake/IntakeState.java +++ b/src/main/java/frc/robot/intake/IntakeState.java @@ -11,6 +11,7 @@ public enum IntakeState { FROM_CONVEYOR, TO_QUEUER, TO_QUEUER_SLOW, + SHUFFLE_ASSIST_WHEN_QUEUER_SENSOR_TURNS_OFF, SHUFFLE, TO_CONVEYOR, TO_QUEUER_SHOOTING; diff --git a/src/main/java/frc/robot/intake/IntakeSubsystem.java b/src/main/java/frc/robot/intake/IntakeSubsystem.java index b3a0dc19..4b5bfe72 100644 --- a/src/main/java/frc/robot/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/intake/IntakeSubsystem.java @@ -66,6 +66,9 @@ public void robotPeriodic() { motor.setVoltage(5); } break; + case SHUFFLE_ASSIST_WHEN_QUEUER_SENSOR_TURNS_OFF: + motor.setVoltage(3); + break; case TO_CONVEYOR: motor.setVoltage(3); // 2 break; diff --git a/src/main/java/frc/robot/note_manager/NoteManager.java b/src/main/java/frc/robot/note_manager/NoteManager.java index 58be06dd..1f189623 100644 --- a/src/main/java/frc/robot/note_manager/NoteManager.java +++ b/src/main/java/frc/robot/note_manager/NoteManager.java @@ -211,28 +211,30 @@ public void robotPeriodic() { case IDLE_IN_QUEUER: if (queuer.hasNote()) { intake.setState(IntakeState.IDLE); + conveyor.setState(ConveyorState.IDLE); } else { - intake.setState(IntakeState.TO_QUEUER_SLOW); + intake.setState(IntakeState.SHUFFLE_ASSIST_WHEN_QUEUER_SENSOR_TURNS_OFF); + conveyor.setState(ConveyorState.SHUFFLE_ASSIST_WHEN_QUEUER_SENSOR_TURNS_OFF); } - conveyor.setState(ConveyorState.IDLE); queuer.setState(QueuerState.INTAKING); break; case IDLE_IN_QUEUER_SHUFFLE: if (shuffleTimeoutTimer.hasElapsed(NOTE_SHUFFLE_TIMEOUT_DURATION)) { if (queuer.hasNote()) { intake.setState(IntakeState.IDLE); + conveyor.setState(ConveyorState.IDLE); } else { - intake.setState(IntakeState.TO_QUEUER_SLOW); + intake.setState(IntakeState.SHUFFLE_ASSIST_WHEN_QUEUER_SENSOR_TURNS_OFF); + conveyor.setState(ConveyorState.SHUFFLE_ASSIST_WHEN_QUEUER_SENSOR_TURNS_OFF); } - conveyor.setState(ConveyorState.IDLE); queuer.setState(QueuerState.INTAKING); } else { if (queuer.hasNote()) { intake.setState(IntakeState.SHUFFLE); conveyor.setState(ConveyorState.SHUFFLE); } else { - intake.setState(IntakeState.TO_QUEUER_SLOW); - conveyor.setState(ConveyorState.INTAKE_TO_QUEUER); + intake.setState(IntakeState.SHUFFLE_ASSIST_WHEN_QUEUER_SENSOR_TURNS_OFF); + conveyor.setState(ConveyorState.SHUFFLE_ASSIST_WHEN_QUEUER_SENSOR_TURNS_OFF); } queuer.setState(QueuerState.SHUFFLE); }