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