created the statemachine's layout

src/main/java/frc/robot/Constants.java

@@ -237,6 +237,15 @@ public static boolean isRedAlliance() {
     public static final Pose2d WORKSHOP_STARTING_POSE = new Pose2d(5.98, 2.60, new Rotation2d(0));
   }
 
+  public static class constElevator {
+
+    public static final Distance CORAL_L1_HEIGHT = Units.Inches.of(0);
+    public static final Distance CORAL_L2_HEIGHT = Units.Inches.of(0);
+    public static final Distance CORAL_L3_HEIGHT = Units.Inches.of(0);
+    public static final Distance CORAL_L4_HEIGHT = Units.Inches.of(0);
+
+  }
+
   public static class constVision {
     /**
      * <p>

src/main/java/frc/robot/RobotContainer.java

@@ -25,7 +25,10 @@ public class RobotContainer {
   private final SN_XboxController conDriver = new SN_XboxController(mapControllers.DRIVER_USB);
 
   private final Drivetrain subDrivetrain = new Drivetrain();
-  private final StateMachine subStateMachine = new StateMachine(subDrivetrain);
+  private final Intake subIntake = new Intake();
+  private final Climber subClimber = new Climber();
+  private final Elevator subElevator = new Elevator();
+  private final StateMachine subStateMachine = new StateMachine(subDrivetrain, subIntake, subClimber, subElevator);
   private final RobotPoses robotPose = new RobotPoses(subDrivetrain);
 
   Command TRY_NONE = Commands.deferredProxy(

src/main/java/frc/robot/commands/States/None.java

@@ -2,8 +2,9 @@
 // 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.commands.states;
+package frc.robot.commands.States;
 
+import frc.robot.subsystems.StateMachine.RobotState;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.*;
 
@@ -22,7 +23,7 @@ public None(StateMachine globalStateMachine) {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    globalStateMachine.setRobotState(StateMachine.RobotState.NONE);
+    globalStateMachine.setRobotState(RobotState.NONE);
   }
 
   // Called every time the scheduler runs while the command is scheduled.

src/main/java/frc/robot/commands/States/climbing/Climbing.java

@@ -0,0 +1,43 @@
+// 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.commands.States.climbing;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Elevator;
+import frc.robot.subsystems.StateMachine;
+import frc.robot.subsystems.StateMachine.RobotState;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class Climbing extends Command {
+  StateMachine globalStateMachine;
+
+  public Climbing(StateMachine globalStateMachine, Elevator subElevator) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    this.globalStateMachine = globalStateMachine;
+    addRequirements(globalStateMachine);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    globalStateMachine.setRobotState(RobotState.CLIMBING);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/States/climbing/PrepClimb.java

@@ -0,0 +1,47 @@
+
+// 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.commands.States.climbing;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Climber;
+import frc.robot.subsystems.Elevator;
+import frc.robot.subsystems.Intake;
+import frc.robot.subsystems.StateMachine;
+import frc.robot.subsystems.StateMachine.RobotState;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class PrepClimb extends Command {
+  /** Creates a new PrepClimb. */
+  StateMachine globalStateMachine;
+
+  public PrepClimb(StateMachine globalStateMachine, Climber subClimber, Intake subIntake, Elevator subElevator) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    this.globalStateMachine = globalStateMachine;
+    addRequirements(globalStateMachine);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    globalStateMachine.setRobotState(RobotState.PREP_CLIMB);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/States/climbing/RetractingClimber.java

@@ -0,0 +1,43 @@
+// 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.commands.States.climbing;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Climber;
+import frc.robot.subsystems.StateMachine;
+import frc.robot.subsystems.StateMachine.RobotState;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class RetractingClimber extends Command {
+  StateMachine globalStateMachine;
+
+  public RetractingClimber(StateMachine globalStateMachine, Climber subClimber) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    this.globalStateMachine = globalStateMachine;
+    addRequirements(globalStateMachine);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    globalStateMachine.setRobotState(RobotState.RETRACTING_CLIMBER);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/States/first_scoring_element/CleanHigh.java

@@ -0,0 +1,42 @@
+// 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.commands.States.first_scoring_element;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.StateMachine;
+import frc.robot.subsystems.StateMachine.RobotState;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class CleanHigh extends Command {
+  StateMachine globalStateMachine;
+
+  public CleanHigh(StateMachine globalStateMachine) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    this.globalStateMachine = globalStateMachine;
+    addRequirements(globalStateMachine);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    globalStateMachine.setRobotState(RobotState.CLEAN_HIGH);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/States/first_scoring_element/CleanLow.java

@@ -0,0 +1,42 @@
+// 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.commands.States.first_scoring_element;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.StateMachine;
+import frc.robot.subsystems.StateMachine.RobotState;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class CleanLow extends Command {
+  StateMachine globalStateMachine;
+
+  public CleanLow(StateMachine globalStateMachine) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    this.globalStateMachine = globalStateMachine;
+    addRequirements(globalStateMachine);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    globalStateMachine.setRobotState(RobotState.CLEAN_LOW);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/States/first_scoring_element/IntakeAlgaeGround.java

@@ -0,0 +1,43 @@
+// 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.commands.States.first_scoring_element;
+
+import frc.robot.subsystems.StateMachine.RobotState;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.StateMachine;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class IntakeAlgaeGround extends Command {
+  /** Creates a new IntakeAlgaeGround. */
+  StateMachine globalStateMachine;
+
+  public IntakeAlgaeGround(StateMachine globalStateMachine) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    this.globalStateMachine = globalStateMachine;
+    addRequirements(globalStateMachine);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    globalStateMachine.setRobotState(RobotState.INTAKE_ALGAE_GROUND);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/States/first_scoring_element/IntakeCoralGround.java

@@ -0,0 +1,42 @@
+// 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.commands.States.first_scoring_element;
+
+import frc.robot.subsystems.StateMachine.RobotState;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.StateMachine;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class IntakeCoralGround extends Command {
+  StateMachine globalStateMachine;
+
+  public IntakeCoralGround(StateMachine globalStateMachine) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    this.globalStateMachine = globalStateMachine;
+    addRequirements(globalStateMachine);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    globalStateMachine.setRobotState(RobotState.INTAKE_CORAL_GROUND);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/States/first_scoring_element/IntakeCoralStation.java

@@ -0,0 +1,42 @@
+// 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.commands.States.first_scoring_element;
+
+import frc.robot.subsystems.StateMachine.RobotState;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.StateMachine;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class IntakeCoralStation extends Command {
+  StateMachine globalStateMachine;
+
+  public IntakeCoralStation(StateMachine globalStateMachine) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    this.globalStateMachine = globalStateMachine;
+    addRequirements(globalStateMachine);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    globalStateMachine.setRobotState(RobotState.INTAKE_CORAL_STATION);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}