diff --git a/src/main/java/frc/robot/subsystems/StateMachine.java b/src/main/java/frc/robot/subsystems/StateMachine.java index d7a1e0e..bf2b31f 100644 --- a/src/main/java/frc/robot/subsystems/StateMachine.java +++ b/src/main/java/frc/robot/subsystems/StateMachine.java @@ -8,7 +8,13 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.commands.states.EjectingHopper; +import frc.robot.commands.states.Intaking; import frc.robot.commands.states.None; +import frc.robot.commands.states.RetractIntake; +import frc.robot.commands.states.ReversingShooter; +import frc.robot.commands.states.*; +import frc.robot.commands.states.preps.*; @Logged public class StateMachine extends SubsystemBase { @@ -22,6 +28,7 @@ public StateMachine() { public void setRobotState(RobotState robotState) { currentRobotState = robotState; + } public RobotState getRobotState() { @@ -33,9 +40,197 @@ public Command tryState(RobotState desiredState) { case NONE: switch (currentRobotState) { case NONE: + case INTAKING: + case REVERSING_SHOOTER: + case SHOOTING: + case EJECTING_HOPPER: return new None(); } break; + case INTAKING: + switch (currentRobotState) { + case NONE: + case RETRACT_INTAKE: + case PREP_ANYWHERE: + case PREP_CORNER: + case PREP_HUB: + case PREP_NEUTRAL_TO_ALLIANCE: + case PREP_OPPONENT_TO_ALLIANCE: + case PREP_TOWER: + case PREP_TRENCH: + return new Intaking(); + } + break; + case SHOOTING: + switch (currentRobotState) { + case RETRACT_INTAKE: + case PREP_ANYWHERE: + case PREP_CORNER: + case PREP_HUB: + case PREP_NEUTRAL_TO_ALLIANCE: + case PREP_OPPONENT_TO_ALLIANCE: + case PREP_TOWER: + case PREP_TRENCH: + return new Shooting(); + } + case RETRACT_INTAKE: + switch (currentRobotState) { + case NONE: + case SHOOTING: + case EJECTING_HOPPER: + case REVERSING_SHOOTER: + case INTAKING: + case PREP_ANYWHERE: + case PREP_CORNER: + case PREP_HUB: + case PREP_NEUTRAL_TO_ALLIANCE: + case PREP_OPPONENT_TO_ALLIANCE: + case PREP_TRENCH: + + return new RetractIntake(); + } + break; + case REVERSING_SHOOTER: + switch (currentRobotState) { + case NONE: + case RETRACT_INTAKE: + case PREP_ANYWHERE: + case PREP_CORNER: + case PREP_HUB: + case PREP_NEUTRAL_TO_ALLIANCE: + case PREP_OPPONENT_TO_ALLIANCE: + case PREP_TOWER: + case PREP_TRENCH: + return new ReversingShooter(); + } + break; + case PREP_ANYWHERE: + switch (currentRobotState) { + case NONE: + case RETRACT_INTAKE: + case PREP_ANYWHERE: + case PREP_CORNER: + case PREP_HUB: + case PREP_NEUTRAL_TO_ALLIANCE: + case PREP_OPPONENT_TO_ALLIANCE: + case PREP_TOWER: + case PREP_TRENCH: + case REVERSING_SHOOTER: + case INTAKING: + return new PrepAnywhere(); + } + break; + case PREP_HUB: + switch (currentRobotState) { + case NONE: + case RETRACT_INTAKE: + case PREP_ANYWHERE: + case PREP_CORNER: + case PREP_HUB: + case PREP_NEUTRAL_TO_ALLIANCE: + case PREP_OPPONENT_TO_ALLIANCE: + case PREP_TOWER: + case PREP_TRENCH: + case REVERSING_SHOOTER: + case INTAKING: + return new PrepHub(); + } + break; + case PREP_CORNER: + switch (currentRobotState) { + case NONE: + case RETRACT_INTAKE: + case PREP_ANYWHERE: + case PREP_CORNER: + case PREP_HUB: + case PREP_NEUTRAL_TO_ALLIANCE: + case PREP_OPPONENT_TO_ALLIANCE: + case PREP_TOWER: + case PREP_TRENCH: + case REVERSING_SHOOTER: + case INTAKING: + return new PrepCorner(); + } + break; + case PREP_NEUTRAL_TO_ALLIANCE: + switch (currentRobotState) { + case NONE: + case RETRACT_INTAKE: + case PREP_ANYWHERE: + case PREP_CORNER: + case PREP_HUB: + case PREP_NEUTRAL_TO_ALLIANCE: + case PREP_OPPONENT_TO_ALLIANCE: + case PREP_TOWER: + case PREP_TRENCH: + case REVERSING_SHOOTER: + case INTAKING: + return new PrepNeutralToAlliance(); + } + break; + case PREP_OPPONENT_TO_ALLIANCE: + switch (currentRobotState) { + case NONE: + case RETRACT_INTAKE: + case PREP_ANYWHERE: + case PREP_CORNER: + case PREP_HUB: + case PREP_NEUTRAL_TO_ALLIANCE: + case PREP_OPPONENT_TO_ALLIANCE: + case PREP_TOWER: + case PREP_TRENCH: + case REVERSING_SHOOTER: + case INTAKING: + return new PrepOpponentToAlliance(); + } + break; + case PREP_TOWER: + switch (currentRobotState) { + case NONE: + case RETRACT_INTAKE: + case PREP_ANYWHERE: + case PREP_CORNER: + case PREP_HUB: + case PREP_NEUTRAL_TO_ALLIANCE: + case PREP_OPPONENT_TO_ALLIANCE: + case PREP_TOWER: + case PREP_TRENCH: + case REVERSING_SHOOTER: + case INTAKING: + return new PrepTower(); + } + break; + case PREP_TRENCH: + switch (currentRobotState) { + case NONE: + case RETRACT_INTAKE: + case PREP_ANYWHERE: + case PREP_CORNER: + case PREP_HUB: + case PREP_NEUTRAL_TO_ALLIANCE: + case PREP_OPPONENT_TO_ALLIANCE: + case PREP_TOWER: + case PREP_TRENCH: + case REVERSING_SHOOTER: + case INTAKING: + return new PrepTrench(); + } + break; + case EJECTING_HOPPER: + switch (currentRobotState) { + case NONE: + case RETRACT_INTAKE: + case PREP_ANYWHERE: + case PREP_CORNER: + case PREP_HUB: + case PREP_NEUTRAL_TO_ALLIANCE: + case PREP_OPPONENT_TO_ALLIANCE: + case PREP_TOWER: + case PREP_TRENCH: + case INTAKING: + return new EjectingHopper(); + } + } return Commands .print("ITS SO OVER D: Invalid State Provided, Blame Eli. Attempted to go to: " + desiredState.toString()