|
1 | 1 | package frc.robot.commands.swerve;
|
2 | 2 |
|
3 | 3 | import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
|
4 |
| -import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; |
5 | 4 | import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
|
6 | 5 | import frc.robot.commands.intake.pivot.IntakePivotSetPositionCommand;
|
7 | 6 | import frc.robot.commands.intake.roller.IntakeRollerIntakeCommand;
|
8 | 7 | import frc.robot.controllers.BaseDriveController;
|
9 | 8 | import frc.robot.subsystems.intake.IntakePivotSubsystem;
|
10 | 9 | import frc.robot.subsystems.intake.IntakeRollerSubsystem;
|
11 |
| -import frc.robot.subsystems.leds.LEDSubsystem; |
12 | 10 | import frc.robot.subsystems.leds.LightBarSubsystem;
|
13 | 11 | import frc.robot.subsystems.swerve.SwerveSubsystem;
|
14 | 12 | import frc.robot.vision.NoteDetectionWrapper;
|
15 | 13 |
|
| 14 | +/** |
| 15 | + * Attempts to intake while moving towards the nearest detected note. |
| 16 | + * This command deploys the intake when run, but does not stow it when complete. |
| 17 | + */ |
16 | 18 | public class AutoIntakeSequence extends SequentialCommandGroup {
|
17 | 19 |
|
| 20 | + /** Constructs an {@link AutoIntakeSequence} using the specified subsystems. */ |
18 | 21 | public AutoIntakeSequence(IntakeRollerSubsystem intakeRollerSubsystem,
|
19 | 22 | IntakePivotSubsystem intakePivotSubsystem,
|
20 | 23 | SwerveSubsystem swerveSubsystem,
|
21 | 24 | NoteDetectionWrapper noteDetector,
|
22 | 25 | BaseDriveController driveController,
|
23 | 26 | LightBarSubsystem lightBarSubsystem) {
|
24 | 27 |
|
25 |
| - addCommands(new IntakePivotSetPositionCommand(intakePivotSubsystem, 1), |
26 |
| - new ParallelCommandGroup( |
27 |
| - new NoteAlignCommand(swerveSubsystem, noteDetector, driveController).until(intakeRollerSubsystem::getFrontSensorValue), |
28 |
| - new IntakeRollerIntakeCommand(intakeRollerSubsystem, lightBarSubsystem) |
29 |
| - ) |
| 28 | + addCommands( |
| 29 | + new IntakePivotSetPositionCommand(intakePivotSubsystem, 1), |
| 30 | + new ParallelCommandGroup( |
| 31 | + new NoteAlignCommand(swerveSubsystem, noteDetector, driveController) |
| 32 | + .until(intakeRollerSubsystem::getFrontSensorValue), |
| 33 | + new IntakeRollerIntakeCommand(intakeRollerSubsystem, lightBarSubsystem) |
| 34 | + ) |
30 | 35 | );
|
31 | 36 | }
|
32 | 37 | }
|
0 commit comments