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.shooter.ShooterSetShoot;
008import com.stuypulse.robot.commands.shooter.ShooterWaitForSpinUp;
009import com.stuypulse.robot.commands.swerve.SwerveDriveXMode;
010import com.stuypulse.robot.commands.swerve.SwerveResetPose;
011import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveAlignToHub;
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 FrontHubShootPreloads extends SequentialCommandGroup {
019    public FrontHubShootPreloads(PathPlannerPath... paths) {
020        addCommands(
021            new SwerveResetPose(paths[0].getStartingHolonomicPose().get()),
022            CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]),
023            new SwerveDriveAlignToHub(), // worth noting that alignment commands don't work so this may not aim correctly
024            new SwerveDriveXMode(),
025            new ShooterWaitForSpinUp(),
026            new ShooterSetShoot(),
027            new HandoffSetForward(),
028            new ParallelDeadlineGroup(
029                new WaitCommand(6.5), // shoot for 6-7 seconds
030                new FeederSetForward(), 
031                new IntakeAgitateFastOnce().repeatedly()
032            )
033        );
034    }
035}