Intake Reference added

src/main/deploy/pathplanner/autos/X. LeftTrench-Neutral-Shoot.auto

@@ -11,9 +11,9 @@
           }
         },
         {
-          "type": "wait",
+          "type": "named",
           "data": {
-            "waitTime": 0
+            "name": "Intake Reference"
           }
         },
         {

src/main/deploy/pathplanner/autos/X. RightTrench-Neutral-Shoot.auto

@@ -10,6 +10,12 @@
             "name": "Intake Extended"
           }
         },
+        {
+          "type": "named",
+          "data": {
+            "name": "Intake Reference"
+          }
+        },
         {
           "type": "path",
           "data": {

src/main/deploy/pathplanner/paths/LeftTrench-Neutral-Shoot.path

@@ -121,11 +121,11 @@
     {
       "fieldPosition": {
         "x": 0.0,
-        "y": 7.0
+        "y": 7.4
       },
       "rotationOffset": 180.0,
       "minWaypointRelativePos": 0.0,
-      "maxWaypointRelativePos": 0.7353266888150656,
+      "maxWaypointRelativePos": 0.6821705426356636,
       "name": "Point Towards Zone"
     }
   ],

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;
     }