diff --git a/src/main/java/frc/robot/commands/CustomizedExecutionCommands.java b/src/main/java/frc/robot/commands/CustomizedExecutionCommands.java new file mode 100644 index 00000000..d05493e2 --- /dev/null +++ b/src/main/java/frc/robot/commands/CustomizedExecutionCommands.java @@ -0,0 +1,55 @@ +package frc.robot.commands; + +import java.util.function.BooleanSupplier; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.WrapperCommand; + + +public class CustomizedExecutionCommands { + + + private static class ConditionalCommandExecution extends WrapperCommand { + + protected BooleanSupplier condition; + + public ConditionalCommandExecution(Command command, BooleanSupplier condition){ + super(command); + this.condition = condition; + } + + @Override + public void execute(){ + if (condition.getAsBoolean()){ + m_command.execute(); + } + } + } + + private static class InitializedConditionalStartCommand extends WrapperCommand { + + protected BooleanSupplier condition; + protected boolean canRun; + + public InitializedConditionalStartCommand(Command command, BooleanSupplier condition){ + super(command); + this.condition = condition; + } + + @Override + public void execute(){ + canRun = canRun || condition.getAsBoolean(); + if (canRun){ + m_command.execute(); + } + } + } + + public static Command executeIf(Command command, BooleanSupplier condition){ + return new ConditionalCommandExecution(command, condition); + } + + public static Command initilizedOnlyIf(Command command, BooleanSupplier condition){ + return new InitializedConditionalStartCommand(command, condition); + } +} diff --git a/src/main/java/frc/robot/commands/drive/ConditionalDelayedPathfindingCommand.java b/src/main/java/frc/robot/commands/drive/ConditionalDelayedPathfindingCommand.java new file mode 100644 index 00000000..0111d393 --- /dev/null +++ b/src/main/java/frc/robot/commands/drive/ConditionalDelayedPathfindingCommand.java @@ -0,0 +1,22 @@ +package frc.robot.commands.drive; + +import java.util.function.BooleanSupplier; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.PathConstraints; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.commands.CustomizedExecutionCommands; + +public class ConditionalDelayedPathfindingCommand { + + public static Command generateCommand(Pose2d pose, PathConstraints constraints, double goalEndVelocity, BooleanSupplier conditionToStart){ + return generateCommand(AutoBuilder.pathfindToPose(pose, constraints, goalEndVelocity), conditionToStart); + } + + public static Command generateCommand(Command pathFindingCommand, BooleanSupplier conditionToStart){ + return CustomizedExecutionCommands.initilizedOnlyIf(pathFindingCommand, conditionToStart); + } + +}