Intake Reference added

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

@@ -36,6 +36,7 @@ import edu.wpi.first.wpilibj2.command.WaitCommand;
 import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc4388.robot.commands.waitSupplier;
 import frc4388.robot.commands.Swerve.StayInPosition;
 import frc4388.robot.commands.alignment.AutoAlign;
 import frc4388.robot.constants.Constants;
@@ -137,6 +138,9 @@ public class RobotContainer {
             new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Idle), m_robotIntake)
         );
 
+        private Command WaitIntakeReference =
+            new WaitUntilCommand(m_robotIntake::intakeAtReference);
+
         private Command RobotShootDriving = new SequentialCommandGroup(
             new RunCommand(() -> 
                 m_robotSwerveDrive.enableRotationOverride(FieldPositions.HUB_POSITION, ShooterConstants.AIM_LEAD_TIME.get(), FieldPositions.HUB_POSITION)
@@ -186,7 +190,7 @@ public class RobotContainer {
             // NamedCommands.registerCommand("Lidar Intake", LidarIntake);
             NamedCommands.registerCommand("Intake Extended", IntakeExtended);
             NamedCommands.registerCommand("Robot Shoot Driving", RobotShootDriving);
-
+            NamedCommands.registerCommand("Intake Reference", WaitIntakeReference);
             NamedCommands.registerCommand("WaitShooter", new WaitUntilCommand(m_robotShooter::isShooterUpToSpeed));
             NamedCommands.registerCommand("AllowShooting", new InstantCommand(() -> m_robotShooter.allowShooting(), m_robotShooter));
             NamedCommands.registerCommand("DenyShooting", new InstantCommand(() -> m_robotShooter.denyShooting(), m_robotShooter));

src/main/java/frc4388/robot/commands/waitIntakeReference.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 edu.wpi.first.wpilibj2.command.Command;
+import frc4388.robot.subsystems.intake.Intake;
+
+/* 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 waitIntakeReference extends Command {
+  /** Creates a new waitIntakeReference. */
+  private Intake intake;
+  public waitIntakeReference(Intake intake) {
+    this.intake = intake;
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // 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 intake.intakeAtReference();
+  }
+}

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/intake/Intake.java

@@ -74,6 +74,10 @@ public class Intake extends SubsystemBase {
         return state.rollerTargetOutput;
     }
 
+    public boolean intakeAtReference() {
+        return state.extendedSoftLimit;
+    }
+
     public double getRollerSpeed() {
         return state.rollerOutput;
     }