add checking current state

Author: FRCTeam3255-Shared-K1-10

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

@@ -31,6 +31,7 @@ public class RobotContainer {
   private final Trigger hasGamePieceTrigger = new Trigger(subStager::getHasGP);
   private final Trigger isGPDetectedTrigger = new Trigger(subHopper::getGamePieceHopper);
 
+  
   // Drive
   public RobotContainer() {
     subDrivetrain.setDefaultCommand(com_Drive);
@@ -46,7 +47,7 @@ private void configureBindings() {
             () -> subStateMachine.tryState(RobotState.INTAKE_GROUND)))
         .onFalse(Commands.deferredProxy(
             () -> subStateMachine.tryState(RobotState.NONE))
-            .unless(isGPDetectedTrigger));
+            .onlyIf(subStateMachine::desiredIsCurrent));
 
     // PrepShooter
     m_driverController.btn_A
@@ -63,34 +64,33 @@ private void configureBindings() {
         .whileTrue(Commands.deferredProxy(
             () -> subStateMachine.tryState(RobotState.EJECT_SHOOTER)))
         .onFalse(Commands.deferredProxy(
-            () -> subStateMachine.tryState(RobotState.NONE)));
+            () -> subStateMachine.tryState(RobotState.NONE)).onlyIf(subStateMachine::desiredIsCurrent));
 
     // EjectIntake
     m_driverController.btn_LeftBumper
         .whileTrue(Commands.deferredProxy(
             () -> subStateMachine.tryState(RobotState.EJECT_INTAKE)))
         .onFalse(Commands.deferredProxy(
-            () -> subStateMachine.tryState(RobotState.NONE)));
+            () -> subStateMachine.tryState(RobotState.NONE)).onlyIf(subStateMachine::desiredIsCurrent));
 
     // Shoot
     m_driverController.btn_RightTrigger
         .onTrue(Commands.deferredProxy(
             () -> subStateMachine.tryState(RobotState.SHOOT)))
         .onFalse(Commands.deferredProxy(
-            () -> subStateMachine.tryState(RobotState.NONE)));
-    
+            () -> subStateMachine.tryState(RobotState.NONE)).onlyIf(subStateMachine::desiredIsCurrent) );
 
     // hasGP
-  
-      hasGamePieceTrigger
-          .whileTrue(Commands.deferredProxy(
-              () -> subStateMachine.tryState(RobotState.HAS_GP)));
 
-    //StopShooter
+    hasGamePieceTrigger
+        .whileTrue(Commands.deferredProxy(
+            () -> subStateMachine.tryState(RobotState.HAS_GP)));
+
+    // StopShooter
     m_driverController.btn_X
-    .onTrue(Commands.deferredProxy(
-() -> subStateMachine.tryState(RobotState.STOP_SHOOTER) ));
-    
+        .onTrue(Commands.deferredProxy(
+            () -> subStateMachine.tryState(RobotState.STOP_SHOOTER)));
+
   }
 
   public Command getAutonomousCommand() {

src/main/java/frc/robot/subsystems/StateMachine.java

@@ -14,6 +14,7 @@
 public class StateMachine extends SubsystemBase {
   /** Creates a new StateMachine. */
   public static RobotState currentState;
+  public static RobotState globalDesiredState;
   Hopper globalHopper;
   Intake globalIntake;
   Shooter globalShooter;
@@ -29,6 +30,7 @@ public StateMachine(Hopper passedHopper, Intake passedIntake, Shooter passedShoo
     globalStager = passedStager;
     globalLED = passedLED;
     currentState = RobotState.NONE;
+    globalDesiredState = currentState;
   }
 
   public enum RobotState {
@@ -51,7 +53,12 @@ public RobotState getState() {
     return currentState;
   }
 
+  public boolean desiredIsCurrent() {
+    return globalDesiredState == currentState;
+  }
+
   public Command tryState(RobotState desiredState) {
+    globalDesiredState = desiredState;
     switch (desiredState) { // check what our desired state is
       case EJECT_INTAKE: // if desired state is eject intake
         switch (currentState) { // check what our current state is
@@ -104,8 +111,8 @@ public Command tryState(RobotState desiredState) {
       case STOP_SHOOTER:
         switch (currentState) {
           case PREP_SHOOTER:
-          return new StopShooter(globalStateMachine, globalShooter);          
-        
+            return new StopShooter(globalStateMachine, globalShooter);
+
         }
         break;
       case NONE:
@@ -118,7 +125,7 @@ public Command tryState(RobotState desiredState) {
             return new none(globalStateMachine, globalHopper, globalIntake, globalShooter, globalStager, globalLED);
         }
         break;
-        
+
       // default:
       // return new None(globalHopper, globalIntake, globalShooter, globalStager);
     }