Skip to content

Commit e2386e4

Browse files
committed
fix path following command requirements
1 parent ec05757 commit e2386e4

File tree

2 files changed

+2
-9
lines changed

2 files changed

+2
-9
lines changed

build.gradle

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -92,7 +92,7 @@ publishing {
9292
gpr(MavenPublication) {
9393
groupId = 'org.carlmontrobotics'
9494
artifactId = 'lib199'
95-
version = '4.0.0'
95+
version = '4.0.1'
9696

9797
from components.java
9898
}

src/main/java/org/carlmontrobotics/lib199/path/SwerveDriveInterface.java

Lines changed: 1 addition & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -21,7 +21,6 @@
2121
import edu.wpi.first.math.trajectory.constraint.SwerveDriveKinematicsConstraint;
2222
import edu.wpi.first.wpilibj2.command.Command;
2323
import edu.wpi.first.wpilibj2.command.CommandBase;
24-
import edu.wpi.first.wpilibj2.command.Subsystem;
2524
import edu.wpi.first.wpilibj2.command.SwerveControllerCommand;
2625

2726
public interface SwerveDriveInterface extends DrivetrainInterface {
@@ -113,20 +112,14 @@ public default Command createPPAutoCommand(PathPlannerTrajectory trajectory, Has
113112
* @return PPSwerveControllerCommand
114113
*/
115114
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
122115
PIDController[] pidControllers = Arrays.stream(getPIDConstants()).map(constants -> new PIDController(constants[0], constants[1], constants[2])).toArray(PIDController[]::new);
123116
pidControllers[2].enableContinuousInput(-Math.PI, Math.PI);
124117
// Use SwerveAutoBuilder because required argument for base auto builder "DrivetrainType" is protected
125118
return new SwerveAutoBuilder(this::getPose, this::setPose, null, null, null, null, eventMap, true) {
126119
@Override
127120
public CommandBase followPath(PathPlannerTrajectory trajectory) {
128121
// 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);
130123
};
131124
}.followPathGroupWithEvents(trajectory);
132125
}

0 commit comments

Comments
 (0)