From 1bdba6d24bd57fab1d95ef0a3ca6247b39e03280 Mon Sep 17 00:00:00 2001
From: McGrathMaggie <78870mm@gmail.com>
Date: Mon, 27 Jan 2025 19:35:08 -0700
Subject: [PATCH] Switch State Statement

---
 .../java/frc4388/robot/RobotContainer.java    |  9 +--
 .../frc4388/robot/subsystems/Elevator.java    | 66 ++++++++++---------
 2 files changed, 41 insertions(+), 34 deletions(-)

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index c4fd224..26b6185 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -47,6 +47,7 @@ import com.pathplanner.lib.commands.PathPlannerAuto;
 // Subsystems
 // import frc4388.robot.subsystems.LED;
 import frc4388.robot.subsystems.Vision;
+import frc4388.robot.subsystems.Elevator.CoordinationState;
 import frc4388.robot.subsystems.Lidar;
 import frc4388.robot.subsystems.Elevator;
 // import frc4388.robot.subsystems.Endeffector;
@@ -216,16 +217,16 @@ public class RobotContainer {
 
         
         DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.A_BUTTON)
-            .onTrue(new InstantCommand(() -> m_robotElevator.transitionWaiting(), m_robotElevator));
+            .onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator));
         
         DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.B_BUTTON)
-            .onTrue(new InstantCommand(() -> m_robotElevator.transitionReady(), m_robotElevator));
+            .onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Ready), m_robotElevator));
         
         DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.Y_BUTTON)
-            .onTrue(new InstantCommand(() -> m_robotElevator.transitionScoringThree(), m_robotElevator));
+            .onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator));
 
         DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.X_BUTTON)
-            .onTrue(new InstantCommand(() -> m_robotElevator.transitionScoringFour(), m_robotElevator));
+            .onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator));
         
             
         // ? /* Programer Buttons (Controller 3)*/
diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java
index 5af17d3..f7fa030 100644
--- a/src/main/java/frc4388/robot/subsystems/Elevator.java
+++ b/src/main/java/frc4388/robot/subsystems/Elevator.java
@@ -21,7 +21,7 @@ public class Elevator extends SubsystemBase {
   private DigitalInput basinLimitSwitch;
   private DigitalInput endefectorLimitSwitch;
 
-  public enum CordinationState {
+  public enum CoordinationState {
     Waiting, // for coral into the though 
     Ready, // Has coral in enefector
     ScoringThree, // Arm and elevator in the level three position
@@ -29,7 +29,7 @@ public class Elevator extends SubsystemBase {
 
   }
 
-  private CordinationState currentState;
+  private CoordinationState currentState;
 
   public Elevator(TalonFX elevatorTalonFX, TalonFX endefectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endefectorLimitSwitch) {
     elevatorMotor = elevatorTalonFX;
@@ -43,7 +43,7 @@ public class Elevator extends SubsystemBase {
     
     elevatorMotor.getConfigurator().apply(ElevatorConstants.ELEVATOR_PID);
     endefectorMotor.getConfigurator().apply(ElevatorConstants.ENDEFECTOR_PID);
-    currentState = CordinationState.Ready;
+    currentState = CoordinationState.Ready;
   }
 
   //PID methods
@@ -60,45 +60,51 @@ public class Elevator extends SubsystemBase {
   public void endefectorStop() {
     endefectorMotor.set(0);
   }
-  
-  public void transitionWaiting() {
-    PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR);
-    PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR);
-    currentState = CordinationState.Waiting;
+
+  public void transitionState(CoordinationState state) {
+    currentState = state;
+    switch (currentState) {
+      case Waiting: {
+        PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR);
+        PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR);
+        break;
+      }
+
+      case Ready: {
+        PIDPosition(elevatorMotor, ElevatorConstants.GROUND_POSITION_ELEVATOR);
+        PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR);
+        break;
+      }
+
+      case ScoringThree: {
+        PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR);
+        PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_TOP_ENDEFECTOR);
+        break;
+      }
+
+      case ScoringFour: {
+        PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR);
+        PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_TOP_ENDEFECTOR);
+        break;
+      }
+    }
+
   }
 
   private void periodicWaiting() {
-    if (basinLimitSwitch.get()) transitionReady();
-  }
-
-  public void transitionReady() {
-    PIDPosition(elevatorMotor, ElevatorConstants.GROUND_POSITION_ELEVATOR);
-    PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR);
-    currentState = CordinationState.Ready;
-  }
-
-  public void transitionScoringThree() {
-    PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR);
-    PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_TOP_ENDEFECTOR);
-    currentState = CordinationState.ScoringThree;
+    if (basinLimitSwitch.get()) transitionState(CoordinationState.Ready);
   }
   
   private void periodicScoring() {
-    if (!endefectorLimitSwitch.get()) transitionWaiting();
-  }
-
-  public void transitionScoringFour() {
-    PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR);
-    PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_TOP_ENDEFECTOR);
-    currentState = CordinationState.ScoringFour;
+    if (!endefectorLimitSwitch.get()) transitionState(CoordinationState.Waiting);
   }
 
   @Override
   public void periodic() {
     // This method will be called once per scheduler run
-    if (currentState == CordinationState.Waiting) {
+    if (currentState == CoordinationState.Waiting) {
       periodicWaiting();
-    } else if (currentState == CordinationState.ScoringThree || currentState == CordinationState.ScoringFour) {
+    } else if (currentState == CoordinationState.ScoringThree || currentState == CoordinationState.ScoringFour) {
       periodicScoring();
     }
   }