001package com.stuypulse.robot.commands.auton.shooting;
002
003import com.pathplanner.lib.path.PathPlannerPath;
004import com.stuypulse.robot.commands.compound.StopShooting;
005import com.stuypulse.robot.commands.compound.TunableWaitCommand;
006import com.stuypulse.robot.commands.feeder.FeederSetForward;
007import com.stuypulse.robot.commands.handoff.HandoffSetForward;
008import com.stuypulse.robot.commands.intake.IntakeAgitateFastOnce;
009import com.stuypulse.robot.commands.intake.IntakeSetIntake;
010import com.stuypulse.robot.commands.shooter.ShooterSetShoot;
011import com.stuypulse.robot.commands.shooter.ShooterWaitForSpinUp;
012import com.stuypulse.robot.commands.swerve.SwerveDriveXMode;
013import com.stuypulse.robot.commands.swerve.SwerveResetPose;
014import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain;
015
016import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
017import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
018import edu.wpi.first.wpilibj2.command.WaitCommand;
019
020public class LBDumpy extends SequentialCommandGroup {
021    public LBDumpy(PathPlannerPath... paths) {
022        addCommands(
023            new SwerveResetPose(paths[0].getStartingHolonomicPose().get()),
024            new TunableWaitCommand("LB Dumpy Delay"),
025            new IntakeSetIntake(),
026            CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]),
027            CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]),
028            CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]),
029            CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]),
030            new SwerveDriveXMode(),
031            new ShooterWaitForSpinUp(),
032            new ShooterSetShoot(),
033            new HandoffSetForward(),
034            new ParallelDeadlineGroup(
035                new WaitCommand(7),
036                new FeederSetForward(), 
037                new IntakeAgitateFastOnce().repeatedly()
038            ),
039            new StopShooting(),
040            new IntakeSetIntake(),
041            CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4])
042        );
043    }
044}