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.ShooterFirstShotIncrease;
009import com.stuypulse.robot.commands.shooter.ShooterSetShoot;
010import com.stuypulse.robot.commands.shooter.ShooterWaitForSpinUp;
011import com.stuypulse.robot.commands.swerve.SwerveDriveXMode;
012import com.stuypulse.robot.commands.swerve.SwerveResetPose;
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 RBDumpy extends SequentialCommandGroup {
020    public RBDumpy(PathPlannerPath... paths) {
021        addCommands( // TODO" check
022            new SwerveResetPose(paths[0].getStartingHolonomicPose().get()),
023            new IntakeSetIntake(),
024            CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]),
025            CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]),
026            CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]),
027            CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]),
028            new SwerveDriveXMode(),
029            new ShooterWaitForSpinUp(),
030            new ShooterSetShoot(),
031            new HandoffSetForward(),
032            new ParallelDeadlineGroup(
033                new WaitCommand(7),
034                new FeederSetForward(), 
035                new IntakeAgitateFastOnce().repeatedly(),
036                new ShooterFirstShotIncrease()
037            )
038        );
039    }
040}