diff --git a/src/main/java/frc/robot/conveyor/ConveyorState.java b/src/main/java/frc/robot/conveyor/ConveyorState.java index 3cd6f29c..7c42be01 100644 --- a/src/main/java/frc/robot/conveyor/ConveyorState.java +++ b/src/main/java/frc/robot/conveyor/ConveyorState.java @@ -8,6 +8,7 @@ public enum ConveyorState { IDLE, INTAKE_TO_SELF, INTAKE_TO_QUEUER, + CONVEYOR_TO_QUEUER, CONVEYOR_TO_INTAKE, QUEUER_TO_INTAKE, WAITING_AMP_SHOT, diff --git a/src/main/java/frc/robot/conveyor/ConveyorSubsystem.java b/src/main/java/frc/robot/conveyor/ConveyorSubsystem.java index af2c963f..1b2b4a95 100644 --- a/src/main/java/frc/robot/conveyor/ConveyorSubsystem.java +++ b/src/main/java/frc/robot/conveyor/ConveyorSubsystem.java @@ -51,6 +51,7 @@ public void enabledPeriodic() { motor.setVoltage(-3); break; case CONVEYOR_TO_INTAKE: + case CONVEYOR_TO_QUEUER: motor.setVoltage(3); break; case AMP_SHOT: diff --git a/src/main/java/frc/robot/note_manager/NoteManager.java b/src/main/java/frc/robot/note_manager/NoteManager.java index 28ccbbf1..b6ac2edb 100644 --- a/src/main/java/frc/robot/note_manager/NoteManager.java +++ b/src/main/java/frc/robot/note_manager/NoteManager.java @@ -47,20 +47,19 @@ public void robotPeriodic() { case AMP_WAIT: if (state == NoteState.IDLE_IN_CONVEYOR) { // Do nothing, you are already idling with the note in the conveyor - } else if (state == NoteState.QUEUER_TO_INTAKE_FOR_CONVEYOR_FINAL - || state == NoteState.INTAKE_TO_CONVEYOR) { + } else if (state == NoteState.QUEUER_TO_CONVEYOR_FOR_IDLE) { // Do nothing, we are already in the handoff process } else { - state = NoteState.QUEUER_TO_INTAKE_FOR_CONVEYOR; + state = NoteState.QUEUER_TO_CONVEYOR_FOR_IDLE; } break; case IDLE_IN_QUEUER: if (state == NoteState.IDLE_IN_CONVEYOR) { - state = NoteState.CONVEYOR_TO_INTAKE_FOR_QUEUER_IDLE; - } else if (state == NoteState.CONVEYOR_TO_INTAKE_FOR_QUEUER_IDLE) { + state = NoteState.CONVEYOR_TO_QUEUER_FOR_IDLE; + } else if (state == NoteState.CONVEYOR_TO_QUEUER_FOR_IDLE) { // Do nothing, we are already in the handoff process } else { - state = NoteState.IDLE_IN_QUEUER; + state = NoteState.CONVEYOR_TO_QUEUER_FOR_IDLE; } break; case IDLE_NO_GP: @@ -78,14 +77,14 @@ public void robotPeriodic() { break; case SHOOTER_OUTTAKE: if (state == NoteState.IDLE_IN_CONVEYOR) { - state = NoteState.CONVEYOR_TO_INTAKE_FOR_SHOOTER_OUTTAKE; + state = NoteState.CONVEYOR_TO_QUEUER_FOR_SHOOTER_OUTTAKE; } else { state = NoteState.SHOOTER_OUTTAKING; } break; case SHOOTER_SCORE: if (state == NoteState.IDLE_IN_CONVEYOR) { - state = NoteState.CONVEYOR_TO_INTAKE_FOR_SHOOTER_SCORE; + state = NoteState.CONVEYOR_TO_QUEUER_FOR_SHOOTER_SCORE; } else { state = NoteState.SHOOTING; } @@ -118,42 +117,36 @@ public void robotPeriodic() { state = NoteState.IDLE_IN_QUEUER; } break; - case INTAKE_TO_CONVEYOR: - if (conveyor.hasNote() && !intake.hasNote()) { - state = NoteState.IDLE_IN_CONVEYOR; - } - break; - case QUEUER_TO_INTAKE_FOR_CONVEYOR: - if (intake.hasNote() && !queuer.hasNote()) { - state = NoteState.QUEUER_TO_INTAKE_FOR_CONVEYOR_FINAL; + case CONVEYOR_TO_INTAKE_FOR_OUTTAKING: + if (!conveyor.hasNote() && intake.hasNote()) { + state = NoteState.OUTTAKING; } break; - case QUEUER_TO_INTAKE_FOR_CONVEYOR_FINAL: - if (!intake.hasNote()) { - state = NoteState.INTAKE_TO_CONVEYOR; + case QUEUER_TO_INTAKE_FOR_OUTTAKING: + if (!queuer.hasNote() && intake.hasNote()) { + state = NoteState.OUTTAKING; } break; - case CONVEYOR_TO_INTAKE_FOR_OUTTAKING: - if (!conveyor.hasNote() && intake.hasNote()) { - state = NoteState.OUTTAKING; + case CONVEYOR_TO_QUEUER_FOR_IDLE: + if (!conveyor.hasNote() && queuer.hasNote()) { + state = NoteState.IDLE_IN_QUEUER; } break; - case CONVEYOR_TO_INTAKE_FOR_QUEUER_IDLE: - if (!conveyor.hasNote() && intake.hasNote()) { - state = NoteState.CONVEYOR_TO_INTAKE_FOR_QUEUER_IDLE_FINAL; + case CONVEYOR_TO_QUEUER_FOR_SHOOTER_OUTTAKE: + if (!conveyor.hasNote() && queuer.hasNote()) { + state = NoteState.SHOOTER_OUTTAKING; } break; - case CONVEYOR_TO_INTAKE_FOR_QUEUER_IDLE_FINAL: - if (!intake.hasNote()) { - state = NoteState.INTAKE_TO_QUEUER; + case CONVEYOR_TO_QUEUER_FOR_SHOOTER_SCORE: + if (!conveyor.hasNote() && queuer.hasNote()) { + state = NoteState.SHOOTING; } break; - case QUEUER_TO_INTAKE_FOR_OUTTAKING: - if (!queuer.hasNote() && intake.hasNote()) { - state = NoteState.OUTTAKING; + case QUEUER_TO_CONVEYOR_FOR_IDLE: + if (!queuer.hasNote() && conveyor.hasNote()) { + state = NoteState.IDLE_IN_CONVEYOR; } break; - // TODO: Fix missing states default: break; } @@ -180,28 +173,11 @@ public void robotPeriodic() { queuer.setState(QueuerState.IDLE); break; case CONVEYOR_TO_INTAKE_FOR_OUTTAKING: - case CONVEYOR_TO_INTAKE_FOR_QUEUER_IDLE: - case CONVEYOR_TO_INTAKE_FOR_QUEUER_IDLE_FINAL: intake.setState(IntakeState.FROM_CONVEYOR); conveyor.setState(ConveyorState.CONVEYOR_TO_INTAKE); queuer.setState(QueuerState.IDLE); break; case OUTTAKING: - intake.setState(IntakeState.OUTTAKING); - conveyor.setState(ConveyorState.QUEUER_TO_INTAKE); - queuer.setState(QueuerState.PASS_TO_INTAKE); - break; - case QUEUER_TO_INTAKE_FOR_CONVEYOR: - case QUEUER_TO_INTAKE_FOR_CONVEYOR_FINAL: - intake.setState(IntakeState.FROM_QUEUER); - conveyor.setState(ConveyorState.QUEUER_TO_INTAKE); - queuer.setState(QueuerState.PASS_TO_INTAKE); - break; - case INTAKE_TO_CONVEYOR: - intake.setState(IntakeState.TO_CONVEYOR); - conveyor.setState(ConveyorState.INTAKE_TO_SELF); - queuer.setState(QueuerState.IDLE); - break; case QUEUER_TO_INTAKE_FOR_OUTTAKING: intake.setState(IntakeState.OUTTAKING); conveyor.setState(ConveyorState.QUEUER_TO_INTAKE); @@ -217,6 +193,22 @@ public void robotPeriodic() { conveyor.setState(ConveyorState.INTAKE_TO_QUEUER); queuer.setState(QueuerState.PASS_TO_SHOOTER); break; + case CONVEYOR_TO_QUEUER_FOR_IDLE: + intake.setState(IntakeState.IDLE); + conveyor.setState(ConveyorState.CONVEYOR_TO_QUEUER); + queuer.setState(QueuerState.INTAKING); + break; + case CONVEYOR_TO_QUEUER_FOR_SHOOTER_OUTTAKE: + case CONVEYOR_TO_QUEUER_FOR_SHOOTER_SCORE: + intake.setState(IntakeState.IDLE); + conveyor.setState(ConveyorState.CONVEYOR_TO_QUEUER); + queuer.setState(QueuerState.PASS_TO_SHOOTER); + break; + case QUEUER_TO_CONVEYOR_FOR_IDLE: + intake.setState(IntakeState.IDLE); + conveyor.setState(ConveyorState.INTAKE_TO_SELF); + queuer.setState(QueuerState.PASS_TO_CONVEYOR); + break; default: break; } diff --git a/src/main/java/frc/robot/note_manager/NoteState.java b/src/main/java/frc/robot/note_manager/NoteState.java index 10e0cfc9..7cffd362 100644 --- a/src/main/java/frc/robot/note_manager/NoteState.java +++ b/src/main/java/frc/robot/note_manager/NoteState.java @@ -10,17 +10,14 @@ public enum NoteState { INTAKE_TO_QUEUER, IDLE_IN_QUEUER, - CONVEYOR_TO_INTAKE_FOR_QUEUER_IDLE, - CONVEYOR_TO_INTAKE_FOR_QUEUER_IDLE_FINAL, - CONVEYOR_TO_INTAKE_FOR_SHOOTER_OUTTAKE, - CONVEYOR_TO_INTAKE_FOR_SHOOTER_SCORE, + CONVEYOR_TO_QUEUER_FOR_IDLE, + CONVEYOR_TO_QUEUER_FOR_SHOOTER_OUTTAKE, + CONVEYOR_TO_QUEUER_FOR_SHOOTER_SCORE, SHOOTING, SHOOTER_OUTTAKING, - QUEUER_TO_INTAKE_FOR_CONVEYOR, - QUEUER_TO_INTAKE_FOR_CONVEYOR_FINAL, - INTAKE_TO_CONVEYOR, + QUEUER_TO_CONVEYOR_FOR_IDLE, IDLE_IN_CONVEYOR, AMP_SCORING, diff --git a/src/main/java/frc/robot/queuer/QueuerState.java b/src/main/java/frc/robot/queuer/QueuerState.java index 654f7109..27b8cee0 100644 --- a/src/main/java/frc/robot/queuer/QueuerState.java +++ b/src/main/java/frc/robot/queuer/QueuerState.java @@ -8,5 +8,6 @@ public enum QueuerState { IDLE, INTAKING, PASS_TO_SHOOTER, + PASS_TO_CONVEYOR, PASS_TO_INTAKE; } diff --git a/src/main/java/frc/robot/queuer/QueuerSubsystem.java b/src/main/java/frc/robot/queuer/QueuerSubsystem.java index b50820f5..7f08ae76 100644 --- a/src/main/java/frc/robot/queuer/QueuerSubsystem.java +++ b/src/main/java/frc/robot/queuer/QueuerSubsystem.java @@ -44,6 +44,7 @@ public void enabledPeriodic() { } break; case PASS_TO_INTAKE: + case PASS_TO_CONVEYOR: motor.setVoltage(-1); break; case PASS_TO_SHOOTER: diff --git a/src/main/java/frc/robot/robot_manager/RobotManager.java b/src/main/java/frc/robot/robot_manager/RobotManager.java index 0c6195e3..38963f06 100644 --- a/src/main/java/frc/robot/robot_manager/RobotManager.java +++ b/src/main/java/frc/robot/robot_manager/RobotManager.java @@ -333,13 +333,6 @@ public void robotPeriodic() { climber.setGoalMode(ClimberMode.IDLE); noteManager.idleNoGPRequest(); break; - case PREPARE_IDLE_WITH_GP_FROM_CONVEYOR: - wrist.setAngle(WristPositions.STOWED); - elevator.setGoalHeight(ElevatorPositions.STOWED); - shooter.setGoalMode(ShooterMode.IDLE); - climber.setGoalMode(ClimberMode.IDLE); - noteManager.ampWaitRequest(); - break; case IDLE_WITH_GP: wrist.setAngle(wristAngleForSpeaker); elevator.setGoalHeight(ElevatorPositions.STOWED); @@ -425,6 +418,7 @@ public void robotPeriodic() { snaps.setEnabled(true); snaps.cancelCurrentCommand(); break; + case PREPARE_IDLE_WITH_GP_FROM_CONVEYOR: case PREPARE_WAITING_AMP_SHOT: wrist.setAngle(WristPositions.STOWED); elevator.setGoalHeight(ElevatorPositions.STOWED);