diff --git a/src/main/java/frc/robot/commands/swerve/AutoIntakeSequence.java b/src/main/java/frc/robot/commands/swerve/AutoIntakeSequence.java index ebdbd018..548d34d2 100644 --- a/src/main/java/frc/robot/commands/swerve/AutoIntakeSequence.java +++ b/src/main/java/frc/robot/commands/swerve/AutoIntakeSequence.java @@ -1,20 +1,23 @@ package frc.robot.commands.swerve; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.commands.intake.pivot.IntakePivotSetPositionCommand; import frc.robot.commands.intake.roller.IntakeRollerIntakeCommand; import frc.robot.controllers.BaseDriveController; import frc.robot.subsystems.intake.IntakePivotSubsystem; import frc.robot.subsystems.intake.IntakeRollerSubsystem; -import frc.robot.subsystems.leds.LEDSubsystem; import frc.robot.subsystems.leds.LightBarSubsystem; import frc.robot.subsystems.swerve.SwerveSubsystem; import frc.robot.vision.NoteDetectionWrapper; +/** + * Attempts to intake while moving towards the nearest detected note. + * This command deploys the intake when run, but does not stow it when complete. + */ public class AutoIntakeSequence extends SequentialCommandGroup { + /** Constructs an {@link AutoIntakeSequence} using the specified subsystems. */ public AutoIntakeSequence(IntakeRollerSubsystem intakeRollerSubsystem, IntakePivotSubsystem intakePivotSubsystem, SwerveSubsystem swerveSubsystem, @@ -22,11 +25,13 @@ public AutoIntakeSequence(IntakeRollerSubsystem intakeRollerSubsystem, BaseDriveController driveController, LightBarSubsystem lightBarSubsystem) { - addCommands(new IntakePivotSetPositionCommand(intakePivotSubsystem, 1), - new ParallelCommandGroup( - new NoteAlignCommand(swerveSubsystem, noteDetector, driveController).until(intakeRollerSubsystem::getFrontSensorValue), - new IntakeRollerIntakeCommand(intakeRollerSubsystem, lightBarSubsystem) - ) + addCommands( + new IntakePivotSetPositionCommand(intakePivotSubsystem, 1), + new ParallelCommandGroup( + new NoteAlignCommand(swerveSubsystem, noteDetector, driveController) + .until(intakeRollerSubsystem::getFrontSensorValue), + new IntakeRollerIntakeCommand(intakeRollerSubsystem, lightBarSubsystem) + ) ); } } diff --git a/src/main/java/frc/robot/commands/swerve/NoteAlignCommand.java b/src/main/java/frc/robot/commands/swerve/NoteAlignCommand.java index ceebd834..c6e75632 100644 --- a/src/main/java/frc/robot/commands/swerve/NoteAlignCommand.java +++ b/src/main/java/frc/robot/commands/swerve/NoteAlignCommand.java @@ -1,7 +1,6 @@ package frc.robot.commands.swerve; import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.controllers.BaseDriveController;