RobotCode2026/src/main/java/frc/robot/commands/ShootPreloadCommand.java at e5e3e7357d0c18a4591f42a11548b0cd4967e21b · Team537/RobotCode2026 · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
package frc.robot.commands;

import java.util.function.Supplier;

import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.WaitCommand;
import frc.robot.subsystems.IntakePivotSubsystem;
import frc.robot.subsystems.IntakeRollerSubsystem;
import frc.robot.subsystems.ShooterSubsystem;
import frc.robot.subsystems.TransferSubsystem;
import frc.robot.subsystems.TurretSubsystem;

public class ShootPreloadCommand extends SequentialCommandGroup {

    public ShootPreloadCommand(
        ShooterSubsystem shooter,
        TurretSubsystem turret,
        TransferSubsystem transfer,
        IntakePivotSubsystem intakePivot,
        IntakeRollerSubsystem intakeRoller,
        Supplier<Translation3d> targetSupplier,
        Supplier<Pose2d> robotPoseSupplier,
        Supplier<ChassisSpeeds> robotVelocitySupplier,
        double shootPreloadTime
    ) {

        addCommands(

            Commands.deadline(
                Commands.waitSeconds(shootPreloadTime + 2),
                new SequentialCommandGroup(
                    new ParallelCommandGroup(
                        turret.getTargetCommand(targetSupplier, robotPoseSupplier, robotVelocitySupplier),
                        shooter.getTargetCommand(targetSupplier, robotPoseSupplier, robotVelocitySupplier),
                        intakeRoller.getIntakeCommand(),
                        intakePivot.deployIntakeCommand()
                    ),
                    new WaitCommand(2),
                    transfer.getLoadCommand()
                )
            ),

            Commands.parallel(
                shooter.getStopCommand(),
                transfer.getStopCommand(),
                intakePivot.raiseIntakeCommand(),
                intakeRoller.getStopCommand()
            ).withTimeout(3.0)

        );

    }

}