Add wrapper command to allow otf pathfinding to start and not start driving

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);
+    }
+}

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);
+    }
+
+}