diff --git a/src/main/java/frc/robot/note_manager/NoteLocation.java b/src/main/java/frc/robot/note_manager/NoteLocation.java deleted file mode 100644 index 628b9bde..00000000 --- a/src/main/java/frc/robot/note_manager/NoteLocation.java +++ /dev/null @@ -1,12 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.note_manager; - -public enum NoteLocation { - NONE, - INTAKE, - CONVEYOR, - QUEUER; -} diff --git a/src/main/java/frc/robot/robot_manager/RobotManager.java b/src/main/java/frc/robot/robot_manager/RobotManager.java index fc450891..f6162871 100644 --- a/src/main/java/frc/robot/robot_manager/RobotManager.java +++ b/src/main/java/frc/robot/robot_manager/RobotManager.java @@ -137,7 +137,7 @@ public void robotPeriodic() { } break; case OUTTAKE: - if (state.homed && state.noteLocation != RobotState.OUTTAKING.noteLocation) { + if (state.homed) { state = RobotState.OUTTAKING; } break; @@ -153,7 +153,7 @@ public void robotPeriodic() { break; case WAIT_AMP_SHOT: if (state.homed) { - state = RobotState.WAITING_AMP_SHOT; + state = RobotState.PREPARE_WAITING_AMP_SHOT; } break; case AMP_SHOT: @@ -216,6 +216,11 @@ public void robotPeriodic() { state = RobotState.IDLE_NO_GP; } break; + case PREPARE_WAITING_AMP_SHOT: + if (noteManager.getState() == NoteState.IDLE_IN_CONVEYOR) { + state = RobotState.WAITING_AMP_SHOT; + } + break; case GROUND_INTAKING: if (noteManager.getState() == NoteState.IDLE_IN_QUEUER) { state = RobotState.IDLE_WITH_GP; @@ -403,6 +408,13 @@ public void robotPeriodic() { snaps.setEnabled(true); snaps.cancelCurrentCommand(); break; + case PREPARE_WAITING_AMP_SHOT: + wrist.setAngle(WristPositions.STOWED); + elevator.setGoalHeight(ElevatorPositions.STOWED); + shooter.setGoalMode(ShooterMode.IDLE); + climber.setGoalMode(ClimberMode.IDLE); + noteManager.ampWaitRequest(); + break; case WAITING_AMP_SHOT: case PREPARE_AMP_SHOT: wrist.setAngle(WristPositions.STOWED); diff --git a/src/main/java/frc/robot/robot_manager/RobotState.java b/src/main/java/frc/robot/robot_manager/RobotState.java index a2642901..771559e8 100644 --- a/src/main/java/frc/robot/robot_manager/RobotState.java +++ b/src/main/java/frc/robot/robot_manager/RobotState.java @@ -4,82 +4,80 @@ package frc.robot.robot_manager; -import frc.robot.note_manager.NoteLocation; - public enum RobotState { /** Not homed. */ - UNHOMED(false, NoteLocation.NONE, false), + UNHOMED(false, false), /** Homing. */ - HOMING(false, NoteLocation.NONE, false), + HOMING(false, false), /** Idling without a note. */ - IDLE_NO_GP(false, NoteLocation.NONE), + IDLE_NO_GP(false), /** Idling with a note in the queuer. */ - IDLE_WITH_GP(true, NoteLocation.QUEUER), + IDLE_WITH_GP(true), /** Intaking a game piece. Transition to INTAKE_TO_QUEUER when done. */ - GROUND_INTAKING(false, NoteLocation.NONE), + GROUND_INTAKING(false), /** Outtaking via the shooter. Game piece should be in queuer at start. */ - OUTTAKING_SHOOTER(true, NoteLocation.QUEUER), + OUTTAKING_SHOOTER(true), /** Outtaking via the intake. Game piece should be in queuer at start. */ - OUTTAKING(true, NoteLocation.INTAKE), + OUTTAKING(true), /** Preparing for floor shot, waiting for driver to commit. */ - WAITING_FLOOR_SHOT(true, NoteLocation.QUEUER), + WAITING_FLOOR_SHOT(true), /** Preparing for floor shot, should shoot when ready. */ - PREPARE_FLOOR_SHOT(true, NoteLocation.QUEUER), + PREPARE_FLOOR_SHOT(true), /** Actively doing the floor shot. */ - FLOOR_SHOOT(true, NoteLocation.QUEUER), + FLOOR_SHOOT(true), /** * Get ready for subwoofer shot, wait for driver/operator (?) to confirm, then go to * PREPARE_SUBWOOFER_SHOT. */ - WAITING_SUBWOOFER_SHOT(true, NoteLocation.QUEUER), + WAITING_SUBWOOFER_SHOT(true), /** Get ready for subwoofer shot, automatically go to SUBWOOFER_SHOOT when ready. */ - PREPARE_SUBWOOFER_SHOT(true, NoteLocation.QUEUER), + PREPARE_SUBWOOFER_SHOT(true), /** Actively doing the subwoofer shot. */ - SUBWOOFER_SHOOT(true, NoteLocation.QUEUER), + SUBWOOFER_SHOOT(true), - PREPARE_TRAP_OUTTAKE(true, NoteLocation.CONVEYOR), - TRAP_OUTTAKE(true, NoteLocation.CONVEYOR), + PREPARE_TRAP_OUTTAKE(true), + TRAP_OUTTAKE(true), /** Get ready for speaker shot, wait for driver to confirm, then go to PREPARE_SPEAKER_SHOT. */ - WAITING_SPEAKER_SHOT(true, NoteLocation.QUEUER), + WAITING_SPEAKER_SHOT(true), /** Get ready for speaker shot, automatically go to SPEAKER_SHOOT when ready. */ - PREPARE_SPEAKER_SHOT(true, NoteLocation.QUEUER), - SPEAKER_SHOOT(true, NoteLocation.QUEUER), + PREPARE_SPEAKER_SHOT(true), + SPEAKER_SHOOT(true), + /** Note maybe in queuer, need to move it to conveyor, and then transition to WAITING_AMP_SHOT. */ + PREPARE_WAITING_AMP_SHOT(true), /** Note in conveyor, waiting for driver to commit to amp score. */ - WAITING_AMP_SHOT(true, NoteLocation.CONVEYOR), + WAITING_AMP_SHOT(true), /** Get ready for amp shot, automatically go to AMP_SHOT when ready. */ - PREPARE_AMP_SHOT(true, NoteLocation.CONVEYOR), + PREPARE_AMP_SHOT(true), /** Actively scoring in the amp. */ - AMP_SHOT(true, NoteLocation.CONVEYOR), + AMP_SHOT(true), /** Arm moves to chain height, climber hooks are touching the chain */ - WAITING_CLIMBER_RAISED(true, NoteLocation.CONVEYOR), + WAITING_CLIMBER_RAISED(true), /** On the ground, arm goes up */ - PREPARE_CLIMBER_RAISED(true, NoteLocation.CONVEYOR), - CLIMBER_RAISED(true, NoteLocation.CONVEYOR), + PREPARE_CLIMBER_RAISED(true), + CLIMBER_RAISED(true), /** Actually climbing then hanging */ - PREPARE_CLIMBER_HANGING(true, NoteLocation.CONVEYOR), - CLIMBER_HANGING(true, NoteLocation.CONVEYOR); + PREPARE_CLIMBER_HANGING(true), + CLIMBER_HANGING(true); public final boolean hasNote; public final boolean homed; - public final NoteLocation noteLocation; - RobotState(boolean hasNote, NoteLocation noteLocation) { - this(hasNote, noteLocation, true); + RobotState(boolean hasNote) { + this(hasNote, true); } - RobotState(boolean hasNote, NoteLocation noteLocation, boolean homed) { + RobotState(boolean hasNote, boolean homed) { this.hasNote = hasNote; - this.noteLocation = noteLocation; this.homed = homed; } }