Intake.java.diff

diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java
index 732b9ad..b5d9299 100644
--- a/src/main/java/frc/robot/subsystems/intake/Intake.java
+++ b/src/main/java/frc/robot/subsystems/intake/Intake.java
@@ -25,6 +25,7 @@ import edu.wpi.first.math.util.Units;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.units.measure.Current;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.RobotContainer;
 import frc.robot.commons.BreadUtil;
@@ -71,6 +72,8 @@ public class Intake extends SubsystemBase {
   private boolean spinIntakeInAuto = true; 
   private boolean forceIdleToZero = false;
 
+  private Timer l1BarButtonHysterisis = new Timer();
+
   LoggedTunableNumber pivotkS = new LoggedTunableNumber("IntakePivot/kS", 0.0);
   LoggedTunableNumber angleOffset = new LoggedTunableNumber("IntakePivot/angleOffset", 0);
   LoggedTunableNumber pivotkV = new LoggedTunableNumber("IntakePivot/kV", 0.0);
@@ -157,6 +160,7 @@ private static final double RIGHT_STICK_RELEASE_DELAY = 0.02; // 20 milliseconds
     HOMING,
     IDLE,
     INTAKE,
+    L1_INTAKE,
     SPIT,
     CLIMB,
     CLIMB_MORE,
@@ -261,31 +265,78 @@ private static final double RIGHT_STICK_RELEASE_DELAY = 0.02; // 20 milliseconds
       }
     } else if (systemState == IntakeState.INTAKE) {
       setPivotAngle(Rotation2d.fromDegrees(0.0 + intakeAngleOffset));
-      
-      if (spinWhenDeployed) {
-        if (RobotContainer.hasGutPiece() && !requestIntake) {
-          setIntakePercent(0);
-        } else {
+      setL1BarPosition(idlePosition.get());
 
-          if (RobotContainer.numpad.getSelectedLevel() == Level.L1 || RobotContainer.driver.getRightStickButton()) {
-            setL1BarPosition(deployedPosition.get());
+      // Driver has now requested the INTAKE to be an L1_INTAKE
+      // Technically this induces a 1 cycle delay for going from
+      // IDLE to L1_INTAKE but I don't think it will be a problem,
+      // and doing this is easier in this already messy code.
+      if (RobotContainer.driver.getRightStickButton()) {
+        nextSystemState = IntakeState.L1_INTAKE;
+      // Actually do the normal coral intake functions
+      } else {
+        if (spinWhenDeployed) {
+          if (RobotContainer.hasGutPiece() && !requestIntake) {
+            setIntakePercent(0);
+          } else {
+            setIntakePercent(NORMAL_INTAKING_SPEED.get());
           }
-
-          setIntakePercent(RobotContainer.numpad.getSelectedLevel() == Level.L1 || RobotContainer.driver.getRightStickButton() ? L1_INTAKING_SPEED.get() : NORMAL_INTAKING_SPEED.get());
-          // setIntakePercent(0.9);
+        } else {
+          setIntakePercent(0);
         }
-      } else {
-        setIntakePercent(0);
       }
 
       if (!requestIntake) {
-        if (RobotContainer.numpad.getSelectedLevel() == Level.L1 || RobotContainer.driver.getRightStickButton()) {
-          nextSystemState = IntakeState.L1_INTAKE_HOLD;
-          requestL1IntakeHold = true;
-        } else {
+        nextSystemState = IntakeState.IDLE;
+        spinWhenDeployed = false;
+      }
+    } else if (systemState == IntakeState.L1_INTAKE) {
+      setL1BarPosition(deployedPosition.get());
+      setIntakePercent(L1_INTAKING_SPEED.get());
+      setPivotAngle(Rotation2d.fromDegrees(0.0 + intakeAngleOffset));
+
+      boolean l1SelectButtonState = RobotContainer.driver.getRightStickButton();
+      // NOTE: These 'stateful' released/pressed checks can only
+      // be used from one place in the code for each button!
+      if (RobotContainer.driver.getRightStickButtonReleased()) {
+        l1BarButtonHysterisis.reset();
+        l1BarButtonHysterisis.start();
+      }
+      // Override the button state to still be pressed if the timer hasn't hit yet
+      if (!l1BarButtonHysterisis.hasElapsed(0.2)) {
+        l1SelectButtonState = true;
+      }
+
+      // If L1 is still requested and intake is released, go to
+      // the hold state
+      if (l1SelectButtonState && !requestIntake) {
+        nextSystemState = IntakeState.L1_INTAKE_HOLD;
+        requestL1IntakeHold = true;
+      // If we've been in L1 intake, but the driver decides to
+      // go back to normal intaking
+      } else if (l1SelectButtonState && requestIntake) {
+        nextSystemState = IntakeState.INTAKE;
+      // Otherwise go back to IDLE, though note that this basically
+      // won't happen given the above two blocks and the timer
+      } else {
+        nextSystemState = IntakeState.IDLE;
+      }
+
+    }  else if (systemState == IntakeState.L1_INTAKE_HOLD) {
+      setPivotAngle(Rotation2d.fromDegrees(l1IntakeHoldAngle.get() + angleOffset.get()));
+      setL1BarPosition(deployedPosition.get());
+      setIntakePercent(l1IntakeHoldSpeed.get());
+      
+      if (!requestL1IntakeHold) {
+          nextSystemState = IntakeState.IDLE;
+      }
+    } else if (systemState == IntakeState.L1_SPIT) {
+      setPivotAngle(Rotation2d.fromDegrees(l1SpitAngle.get()));
+      setL1BarPosition(deployedPosition.get());
+      setIntakePercent(l1SpitSpeed.get());
+      
+      if (!requestL1Spit) {
           nextSystemState = IntakeState.IDLE;
-          spinWhenDeployed = false;
-        }
       }
     } else if (systemState == IntakeState.SPIT) {
       setIntakePercent(-0.9);
@@ -327,24 +378,7 @@ private static final double RIGHT_STICK_RELEASE_DELAY = 0.02; // 20 milliseconds
       if (!requestNet) {
         nextSystemState = IntakeState.IDLE;
       }
-    }  else if (systemState == IntakeState.L1_INTAKE_HOLD) {
-      setPivotAngle(Rotation2d.fromDegrees(l1IntakeHoldAngle.get() + angleOffset.get()));
-      setL1BarPosition(deployedPosition.get());
-      setIntakePercent(l1IntakeHoldSpeed.get());
-      
-      if (!requestL1IntakeHold) {
-          nextSystemState = IntakeState.IDLE;
-      }
-  } 
-  else if (systemState == IntakeState.L1_SPIT) {
-      setPivotAngle(Rotation2d.fromDegrees(l1SpitAngle.get()));
-      setL1BarPosition(deployedPosition.get());
-      setIntakePercent(l1SpitSpeed.get());
-      
-      if (!requestL1Spit) {
-          nextSystemState = IntakeState.IDLE;
-      }
-  }
+    }
 
     if (systemState != nextSystemState) {
       stateStartTime = BreadUtil.getFPGATimeSeconds();