From 08800806c09806420622dae5dbd19b87cbd720c6 Mon Sep 17 00:00:00 2001 From: YHerm <0549405422.yh@gmail.com> Date: Tue, 1 Apr 2025 18:07:56 +0300 Subject: [PATCH 1/2] follow path command --- .../robot/visualizers/FollowPathCommand.java | 44 +++++++++++++++++++ 1 file changed, 44 insertions(+) create mode 100644 src/main/java/frc/robot/visualizers/FollowPathCommand.java 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..4b10cdf84c --- /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; + } + +} From f9e1c901f2d4feded8eb94384ebe001eba368458 Mon Sep 17 00:00:00 2001 From: YHerm <0549405422.yh@gmail.com> Date: Tue, 1 Apr 2025 18:16:29 +0300 Subject: [PATCH 2/2] format --- .../robot/visualizers/FollowPathCommand.java | 64 +++++++++---------- 1 file changed, 32 insertions(+), 32 deletions(-) diff --git a/src/main/java/frc/robot/visualizers/FollowPathCommand.java b/src/main/java/frc/robot/visualizers/FollowPathCommand.java index 4b10cdf84c..3b69464f39 100644 --- a/src/main/java/frc/robot/visualizers/FollowPathCommand.java +++ b/src/main/java/frc/robot/visualizers/FollowPathCommand.java @@ -8,37 +8,37 @@ 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; - } + 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; + } }