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.ShooterFirstShotIncrease;
011import com.stuypulse.robot.commands.shooter.ShooterSetShoot;
012import com.stuypulse.robot.commands.shooter.ShooterWaitForSpinUp;
013import com.stuypulse.robot.commands.swerve.SwerveDriveXMode;
014import com.stuypulse.robot.commands.swerve.SwerveResetPose;
015import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain;
016
017import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
018import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
019import edu.wpi.first.wpilibj2.command.WaitCommand;
020
021public class LBDumpy extends SequentialCommandGroup {
022    public LBDumpy(PathPlannerPath... paths) {
023        addCommands(
024            new SwerveResetPose(paths[0].getStartingHolonomicPose().get()),
025            new TunableWaitCommand("LB Dumpy Delay"),
026            new IntakeSetIntake(),
027            CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]),
028            CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]),
029            CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]),
030            CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]),
031            new SwerveDriveXMode(),
032            new ShooterWaitForSpinUp(),
033            new ShooterSetShoot(),
034            new HandoffSetForward(),
035            new ParallelDeadlineGroup(
036                new WaitCommand(7),
037                new FeederSetForward(), 
038                new IntakeAgitateFastOnce().repeatedly(),
039                new ShooterFirstShotIncrease()
040            ),
041            new StopShooting(),
042            new IntakeSetIntake()//,
043            // CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4])
044        );
045    }
046}