factored out amp preparation sequence

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

@@ -48,6 +48,7 @@
 import frc.robot.commands.intake.roller.IntakeRollerAmpIntakeCommand;
 import frc.robot.commands.intake.roller.IntakeRollerIntakeCommand;
 import frc.robot.commands.intake.roller.IntakeRollerOuttakeCommand;
+import frc.robot.commands.sequences.PrepareAmpSequence;
 import frc.robot.commands.shooter.flywheel.ShooterFlywheelShuttleCommand;
 import frc.robot.commands.swerve.AlignCommand;
 import frc.robot.commands.swerve.AutoIntakeSequence;
@@ -418,20 +419,12 @@ private void configureBindings() {
                     // if elevator is up
                     new ElevatorToZeroCommand(elevatorSubsystem).alongWith(new InstantCommand(// lower the elevator
                         () -> intakePivotSubsystem.setPosition(0), intakePivotSubsystem)), // stow the pivot
+
                     // if elevator is down
-                    new SequentialCommandGroup(
-                        new IntakePivotSetPositionCommand(intakePivotSubsystem, 1).unless(() -> !intakeRollerSubsystem.getRockwellSensorValue()).andThen(// extend pivot
-                            new IntakeRollerOuttakeCommand(intakeRollerSubsystem, .17, .75).unless(intakeRollerSubsystem::getAmpSensor) // run rollers to front sensor
-                                    .until(() -> intakeRollerSubsystem.getFrontSensorReached()),
-                        new IntakePivotSetPositionCommand(intakePivotSubsystem, 0.2),
-                        new ElevatorToAmpCommand(elevatorSubsystem)
-                    )
-
-                         // raise elevator
-                         // angle intake for scoring
-                    ).until(() -> mechController.getLeftTriggerAxis() > .05 
-                        || mechController.getRightTriggerAxis() > .05
-                    ), 
+                    new PrepareAmpSequence(elevatorSubsystem, intakePivotSubsystem, intakeRollerSubsystem)
+                        .until(() -> mechController.getLeftTriggerAxis() > .05 
+                                  || mechController.getRightTriggerAxis() > .05),
+
                     // check if the elevator is currently targeting one of the upper positions to choose what to do
                     () -> elevatorSubsystem.getTargetState() == ElevatorState.AMP
                         || elevatorSubsystem.getTargetState() == ElevatorState.TRAP));

src/main/java/frc/robot/commands/sequences/PrepareAmpSequence.java

@@ -0,0 +1,36 @@
+package frc.robot.commands.sequences;
+
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.commands.elevator.ElevatorToAmpCommand;
+import frc.robot.commands.intake.pivot.IntakePivotSetPositionCommand;
+import frc.robot.commands.intake.roller.IntakeRollerOuttakeCommand;
+import frc.robot.subsystems.elevator.ElevatorSubsystem;
+import frc.robot.subsystems.intake.IntakePivotSubsystem;
+import frc.robot.subsystems.intake.IntakeRollerSubsystem;
+
+/**
+ * Deploys the intake, rolls the note to its amping position, then raises the elevator to amp height while putting the
+ * intake at amping angle. Given that a note is already in the robot, this sequence prepares the mechanisms to deposit
+ * it into the amp.
+ */
+public class PrepareAmpSequence extends SequentialCommandGroup {
+
+    /** Constructs a new {@link PrepareAmpSequence}. */
+    public PrepareAmpSequence(ElevatorSubsystem elevatorSubsystem,
+                              IntakePivotSubsystem intakePivotSubsystem,
+                              IntakeRollerSubsystem intakeRollerSubsystem) {
+
+        this.addCommands(
+            new IntakePivotSetPositionCommand(intakePivotSubsystem, 1) // extend pivot
+                .unless(() -> !intakeRollerSubsystem.getRockwellSensorValue()), 
+            new IntakeRollerOuttakeCommand(intakeRollerSubsystem, .17, .75) // run rollers to front sensor
+                .unless(intakeRollerSubsystem::getAmpSensor)
+                .until(() -> intakeRollerSubsystem.getFrontSensorReached()),
+            Commands.parallel(
+                new IntakePivotSetPositionCommand(intakePivotSubsystem, 0.2), // angle intake for scoring
+                new ElevatorToAmpCommand(elevatorSubsystem) // raise elevator
+            ) // These commands can run at the same time to save time.
+        );
+    }
+}