i

src/main/deploy/pathplanner/autos/2 Coral Cage 1.auto

@@ -4,6 +4,12 @@
     "type": "sequential",
     "data": {
       "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "prepare-l4"
+          }
+        },
         {
           "type": "path",
           "data": {
@@ -40,6 +46,12 @@
             "pathName": "Top Feed to Reef 6"
           }
         },
+        {
+          "type": "named",
+          "data": {
+            "name": "await-coral"
+          }
+        },
         {
           "type": "named",
           "data": {

src/main/deploy/pathplanner/autos/2 Coral Cage 5.auto

@@ -4,6 +4,12 @@
     "type": "sequential",
     "data": {
       "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "prepare-l4"
+          }
+        },
         {
           "type": "path",
           "data": {
@@ -40,6 +46,12 @@
             "pathName": "Bottom Feed to Reef 2"
           }
         },
+        {
+          "type": "named",
+          "data": {
+            "name": "await-coral"
+          }
+        },
         {
           "type": "named",
           "data": {

src/main/java/frc4388/robot/Constants.java

@@ -427,6 +427,8 @@ public final class Constants {
         
         public static final int BASIN_LIMIT_SWITCH = 8; // TODO: FIND
         public static final int ENDEFFECTOR_LIMIT_SWITCH = 9; // TODO: FIND
+        public static final int INTAKE_LIMIT_SWITCH = 6; // TODO: FIND
+
         
         public static final double GEAR_RATIO_ELEVATOR = -9.0;
         //Max for elevator = 50%

src/main/java/frc4388/robot/RobotContainer.java

@@ -49,6 +49,7 @@ import frc4388.robot.commands.MoveForTimeCommand;
 import frc4388.robot.commands.waitElevatorRefrence;
 import frc4388.robot.commands.waitEndefectorRefrence;
 import frc4388.robot.commands.waitFeedCoral;
+import frc4388.robot.commands.waitSupplier;
 import frc4388.robot.commands.Swerve.neoJoystickPlayback;
 import frc4388.robot.commands.Swerve.neoJoystickRecorder;
 
@@ -89,7 +90,7 @@ public class RobotContainer {
     public final LED m_robotLED = new LED();
     public final Vision m_vision = new Vision(m_robotMap.leftCamera, m_robotMap.rightCamera);
     public final Lidar m_lidar = new Lidar();
-    public final Elevator m_robotElevator = new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endeffectorLimitSwitch, m_vision, m_robotLED);
+    public final Elevator m_robotElevator = new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endeffectorLimitSwitch, m_robotMap.basinLimitSwitch, m_robotLED);
     public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision);
     // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain);
 
@@ -108,7 +109,7 @@ public class RobotContainer {
 
     // ! Teleop Commands
     public void stop() {
-        new InstantCommand(()->{}, m_robotSwerveDrive).schedule();;;;;
+        new InstantCommand(()->{}, m_robotSwerveDrive).schedule();
         m_robotSwerveDrive.stopModules();
     }
 
@@ -117,6 +118,8 @@ public class RobotContainer {
     private SendableChooser<String> autoChooser;
     private Command autoCommand;
 
+    private Command waitFeedStation = new waitSupplier(m_robotElevator::readyToMove);
+
     // private ConfigurableString autoplaybackName = new ConfigurableString("Auto Playback Name", lastAutoName);
     // private neoJoystickPlayback autoPlayback = new neoJoystickPlayback(m_robotSwerveDrive, 
     // () -> autoplaybackName.get(), // lastAutoName
@@ -316,7 +319,9 @@ public class RobotContainer {
      */
     public RobotContainer() {
 
-        NamedCommands.registerCommand("grab-coral", new waitFeedCoral(m_robotElevator));
+        NamedCommands.registerCommand("grab-coral", waitFeedStation.asProxy());
+        NamedCommands.registerCommand("await-coral", new waitFeedCoral(m_robotElevator));
+
         NamedCommands.registerCommand("align-feed", new SequentialCommandGroup(
             new MoveForTimeCommand(m_robotSwerveDrive, 
                 new Translation2d(0, 1), 
@@ -330,8 +335,8 @@ public class RobotContainer {
         NamedCommands.registerCommand("place-coral-left-l2", AprilLidarAlignL2Left);
         NamedCommands.registerCommand("place-coral-right-l2", AprilLidarAlignL2Right);
 
-        NamedCommands.registerCommand("enable-preraise-l4", new InstantCommand(() -> {
+        NamedCommands.registerCommand("prepare-l4", new InstantCommand(() -> {
-            m_robotElevator.enablePreRaiseElevator();
+            m_robotElevator.transitionState(CoordinationState.PrimedFour);
         }));
 
         configureButtonBindings();        

src/main/java/frc4388/robot/RobotMap.java

@@ -58,6 +58,7 @@ public class RobotMap {
 
     public final DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH);
     public final DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH);
+    public final DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH);
 
     void configureDriveMotorControllers() {}
         

src/main/java/frc4388/robot/commands/waitSupplier.java

@@ -0,0 +1,36 @@
+// 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 frc4388.robot.commands;
+
+import java.util.function.Supplier;
+
+import edu.wpi.first.wpilibj2.command.Command;
+
+/* 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 waitSupplier extends Command {
+  /** Creates a new waitSupplier. */
+  private final Supplier<Boolean> truth;
+  public waitSupplier(Supplier<Boolean> truth) {
+    this.truth = truth;
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // 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 truth.get();
+  }
+}

src/main/java/frc4388/robot/subsystems/Elevator.java

@@ -49,7 +49,7 @@ public class Elevator extends Subsystem {
 
   private DigitalInput basinBeamBreak;
   private DigitalInput endeffectorLimitSwitch;
-  // private DigitalInput intakeIR;
+  private DigitalInput intakeIR;
 
   public enum CoordinationState {
     Waiting, // for coral into the though
@@ -69,15 +69,15 @@ public class Elevator extends Subsystem {
 
   private CoordinationState currentState;
 
-  public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, LED led) {
+  // public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, LED led) {
-  // public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, DigitalInput intakeDigitalInput, LED led) {
+  public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, DigitalInput intakeDigitalInput, LED led) {
     elevatorMotor = elevatorTalonFX;
     endeffectorMotor = endeffectorTalonFX;
     this.led = led;
 
     this.basinBeamBreak = basinLimitSwitch;
     this.endeffectorLimitSwitch = endeffectorLimitSwitch;
-    // this.intakeIR = intakeDigitalInput;
+    this.intakeIR = intakeDigitalInput;
 
     elevatorMotor.setNeutralMode(NeutralModeValue.Brake);
     endeffectorMotor.setNeutralMode(NeutralModeValue.Brake);
@@ -294,6 +294,7 @@ public class Elevator extends Subsystem {
     // This method will be called once per scheduler run
     SmartDashboard.putNumber("Basin", basinBeamBreak.get() ? 1 : 0);
     SmartDashboard.putNumber("endefector", endeffectorLimitSwitch.get() ? 1 : 0);
+    SmartDashboard.putNumber("intake", intakeIR.get() ? 1 : 0);
     SmartDashboard.putString("State", currentState.toString());
     
     if (disableAutoIntake) return;
@@ -310,18 +311,23 @@ public class Elevator extends Subsystem {
     // }
   }
 
-  public boolean isL4Primed(){
+  public boolean isL4Primed() {
     return currentState == CoordinationState.PrimedFour;
   }
 
-  public boolean isL3Primed(){
+  public boolean isL3Primed() {
     return currentState == CoordinationState.PrimedThree;
   }
 
-  public boolean hasCoral(){
+  public boolean hasCoral() {
     return elevatorAtReference() && currentState == CoordinationState.Hovering && !endeffectorLimitSwitch.get();
   }
 
+  public boolean readyToMove() {
+    // return !intakeIR.get() || hasCoral();
+    return hasCoral();
+  }
+
   public void armShuffle(){
     if(!basinBeamBreak.get()){
       //shuffle the coral with the arm until coral hits beam break