001package com.stuypulse.robot.commands.auton.depot;
002
003import com.pathplanner.lib.path.PathPlannerPath;
004import com.stuypulse.robot.commands.feeder.FeederSetForward;
005import com.stuypulse.robot.commands.handoff.HandoffSetForward;
006import com.stuypulse.robot.commands.intake.IntakeAgitateFastOnce;
007import com.stuypulse.robot.commands.intake.IntakeSetIntake;
008import com.stuypulse.robot.commands.shooter.ShooterSetShoot;
009import com.stuypulse.robot.commands.shooter.ShooterWaitForSpinUp;
010import com.stuypulse.robot.commands.swerve.SwerveDriveXMode;
011import com.stuypulse.robot.commands.swerve.SwerveResetPose;
012import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveAlignToHub;
013import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain;
014
015import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
016import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
017import edu.wpi.first.wpilibj2.command.WaitCommand;
018
019public class CenterDepot extends SequentialCommandGroup {
020    public CenterDepot(PathPlannerPath... paths) {
021        addCommands(
022           new SwerveResetPose(paths[0].getStartingHolonomicPose().get()),
023           new IntakeSetIntake(),
024           CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]),
025           CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]),
026           new SwerveDriveXMode(),
027           new ShooterWaitForSpinUp(),
028           new ShooterSetShoot(),
029           new HandoffSetForward(),
030           new ParallelDeadlineGroup(
031            new WaitCommand(5),
032               new FeederSetForward(),
033               new IntakeAgitateFastOnce().repeatedly()
034            )
035        );
036    }
037}