001package com.stuypulse.robot.commands.auton.shooting;
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.subsystems.swerve.CommandSwerveDrivetrain;
013
014import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
015import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
016import edu.wpi.first.wpilibj2.command.WaitCommand;
017
018public class RBDumpy extends SequentialCommandGroup {
019    public RBDumpy(PathPlannerPath... paths) {
020        addCommands( // TODO" check
021            new SwerveResetPose(paths[0].getStartingHolonomicPose().get()),
022            new IntakeSetIntake(),
023            CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]),
024            CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]),
025            CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]),
026            CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]),
027            new SwerveDriveXMode(),
028            new ShooterWaitForSpinUp(),
029            new ShooterSetShoot(),
030            new HandoffSetForward(),
031            new ParallelDeadlineGroup(
032                new WaitCommand(7),
033                new FeederSetForward(), 
034                new IntakeAgitateFastOnce().repeatedly()
035            )
036        );
037    }
038}