|
21 | 21 | import edu.wpi.first.math.trajectory.constraint.SwerveDriveKinematicsConstraint; |
22 | 22 | import edu.wpi.first.wpilibj2.command.Command; |
23 | 23 | import edu.wpi.first.wpilibj2.command.CommandBase; |
24 | | -import edu.wpi.first.wpilibj2.command.Subsystem; |
25 | 24 | import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; |
26 | 25 |
|
27 | 26 | public interface SwerveDriveInterface extends DrivetrainInterface { |
@@ -113,20 +112,14 @@ public default Command createPPAutoCommand(PathPlannerTrajectory trajectory, Has |
113 | 112 | * @return PPSwerveControllerCommand |
114 | 113 | */ |
115 | 114 | public default Command createPPAutoCommand(List<PathPlannerTrajectory> trajectory, HashMap<String, Command> eventMap) { |
116 | | - Subsystem[] requirements = eventMap.values().stream().flatMap(command -> command.getRequirements().stream()) |
117 | | - .toArray(Subsystem[]::new); |
118 | | - requirements = Arrays.copyOf(requirements, requirements.length + 1); |
119 | | - requirements[requirements.length - 1] = this; |
120 | | - requirements = Arrays.stream(requirements).distinct().toArray(Subsystem[]::new); |
121 | | - final Subsystem[] finalRequirements = requirements; // Make this final so it can be used in the anonymous inner class |
122 | 115 | PIDController[] pidControllers = Arrays.stream(getPIDConstants()).map(constants -> new PIDController(constants[0], constants[1], constants[2])).toArray(PIDController[]::new); |
123 | 116 | pidControllers[2].enableContinuousInput(-Math.PI, Math.PI); |
124 | 117 | // Use SwerveAutoBuilder because required argument for base auto builder "DrivetrainType" is protected |
125 | 118 | return new SwerveAutoBuilder(this::getPose, this::setPose, null, null, null, null, eventMap, true) { |
126 | 119 | @Override |
127 | 120 | public CommandBase followPath(PathPlannerTrajectory trajectory) { |
128 | 121 | // AutoBuilder will convert this to work with events |
129 | | - return new PPSwerveControllerCommand(trajectory, poseSupplier, getKinematics(), pidControllers[0], pidControllers[1], pidControllers[2], SwerveDriveInterface.this::drive, true, finalRequirements /* This will propagate the requirements to the final command via SequentialCommandGroup */); |
| 122 | + return new PPSwerveControllerCommand(trajectory, poseSupplier, getKinematics(), pidControllers[0], pidControllers[1], pidControllers[2], SwerveDriveInterface.this::drive, true, SwerveDriveInterface.this); |
130 | 123 | }; |
131 | 124 | }.followPathGroupWithEvents(trajectory); |
132 | 125 | } |
|
0 commit comments