Add safety to prevent note come down from AMP

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

@@ -229,6 +229,10 @@ private void configureButtonBindings() {
         operator.rightTrigger().and(operator.leftTrigger()).whileTrue(intake.runIndexerMotor(1));
         // set shooter to home preset position
         operator.y().onTrue(elevatorWrist.homePosition());
+        operator.y().and(elevatorWrist.elevatorAtAmp).and(noteInIndexer)
+            .onTrue(intake.runIntakeMotorNonStop(0, -0.2).withTimeout(2.0)
+                .until(new Trigger(() -> !this.intake.getIndexerBeamBrakeStatus()).debounce(.5)));
+
         // increment once through states list to next state
         operator.povRight().onTrue(Commands.runOnce(() -> {
             OperatorState.increment();

src/main/java/frc/robot/subsystems/elevator_wrist/ElevatorWrist.java

@@ -20,6 +20,7 @@
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.lib.util.FieldConstants;
 import frc.robot.Constants;
 import frc.robot.OperatorState;
@@ -69,6 +70,8 @@ public class ElevatorWrist extends SubsystemBase {
 
     private double estimatedWristAngle = 0;
 
+    public Trigger elevatorAtAmp = new Trigger(() -> elevatorAtAmp());
+
     /**
      * Create new ElevatorWrist.
      */
@@ -392,4 +395,14 @@ public boolean elevatorAtHome() {
             3);
     }
 
+    /**
+     * Check if the elevator is at the AMP position
+     *
+     * @return True if the elevator is AMP
+     */
+    public boolean elevatorAtAmp() {
+        return MathUtil.isNear(Constants.ElevatorWristConstants.SetPoints.AMP_HEIGHT, getHeight(),
+            3);
+    }
+
 }