Avoid jamming during shuffle

team581 · Apr 19, 2024 · e9d06ce · e9d06ce
1 parent 4d97793
commit e9d06ce
Show file tree

Hide file tree

Showing 5 changed files with 16 additions and 6 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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);
         }