001package com.stuypulse.robot.commands.auton;
002
003import com.pathplanner.lib.path.PathPlannerPath;
004import com.stuypulse.robot.commands.intake.IntakeSetHomingDown;
005import com.stuypulse.robot.commands.swerve.SwerveResetPose;
006import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain;
007import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
008import edu.wpi.first.wpilibj2.command.WaitCommand;
009
010public class LBFerry extends SequentialCommandGroup {
011
012    public LBFerry(PathPlannerPath... paths) {
013        addCommands(
014                new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), CommandSwerveDrivetrain.getInstance()
015                        .followPathCommand(paths[0]),
016                CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]), new WaitCommand(2),
017                CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]).alongWith(new IntakeSetHomingDown()),
018                CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]));
019    }
020}