diff --git a/src/main/java/frc/robot/visualizers/FollowPathCommand.java b/src/main/java/frc/robot/visualizers/FollowPathCommand.java new file mode 100644 index 0000000000..3b69464f39 --- /dev/null +++ b/src/main/java/frc/robot/visualizers/FollowPathCommand.java @@ -0,0 +1,44 @@ +package frc.robot.visualizers; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc.utils.time.TimeUtil; + +import java.util.function.Consumer; + +public class FollowPathCommand extends Command { + + private final Consumer setPosition; + private final double delayBetweenPoints; + private final Translation2d[] points; + + private double setPositionTime = 0; + private int currentPoint = 1; + + public FollowPathCommand(Consumer setPosition, double delayBetweenPoints, Translation2d... points) { + this.setPosition = setPosition; + this.delayBetweenPoints = delayBetweenPoints; + this.points = points; + } + + @Override + public void initialize() { + setPositionTime = TimeUtil.getCurrentTimeSeconds(); + currentPoint = 1; + } + + @Override + public void execute() { + if (TimeUtil.getCurrentTimeSeconds() - setPositionTime >= delayBetweenPoints) { + setPosition.accept(points[currentPoint]); + currentPoint++; + setPositionTime = TimeUtil.getCurrentTimeSeconds(); + } + } + + @Override + public boolean isFinished() { + return currentPoint >= points.length; + } + +}