From e31f36b91aa8ff18298e0f461ec1468be163871f Mon Sep 17 00:00:00 2001 From: Zach R Date: Sat, 10 Feb 2024 16:46:41 -0800 Subject: [PATCH] refactor: restructure PathPlannerHelper --- .../commands/autos/AmpLanePABCSprint.java | 21 +++-- .../commands/autos/SourceLanePHGFSprint.java | 18 ++-- .../team1540/robot2024/util/AutoCommand.java | 22 ++--- .../team1540/robot2024/util/PathHelper.java | 86 +++++++++++++++++++ .../robot2024/util/PathPlannerHelper.java | 75 ---------------- 5 files changed, 111 insertions(+), 111 deletions(-) create mode 100644 src/main/java/org/team1540/robot2024/util/PathHelper.java delete mode 100644 src/main/java/org/team1540/robot2024/util/PathPlannerHelper.java diff --git a/src/main/java/org/team1540/robot2024/commands/autos/AmpLanePABCSprint.java b/src/main/java/org/team1540/robot2024/commands/autos/AmpLanePABCSprint.java index b627e826..74cb900e 100644 --- a/src/main/java/org/team1540/robot2024/commands/autos/AmpLanePABCSprint.java +++ b/src/main/java/org/team1540/robot2024/commands/autos/AmpLanePABCSprint.java @@ -1,11 +1,10 @@ package org.team1540.robot2024.commands.autos; -import edu.wpi.first.wpilibj2.command.PrintCommand; import org.team1540.robot2024.subsystems.drive.Drivetrain; import org.team1540.robot2024.subsystems.indexer.Indexer; import org.team1540.robot2024.subsystems.shooter.Shooter; import org.team1540.robot2024.util.AutoCommand; -import org.team1540.robot2024.util.PathPlannerHelper; +import org.team1540.robot2024.util.PathHelper; public class AmpLanePABCSprint extends AutoCommand { @@ -13,20 +12,20 @@ public AmpLanePABCSprint(Drivetrain drivetrain, Shooter shooter, Indexer indexer super("AmpLanePABCSprint"); addPath( - new PathPlannerHelper(drivetrain, "AmpLanePABCSprint.1", true, true, true), - new PathPlannerHelper(drivetrain, "AmpLanePABCSprint.2", true), - new PathPlannerHelper(drivetrain, "AmpLanePABCSprint.3", true), - new PathPlannerHelper(drivetrain, "AmpLanePABCSprint.4", true) - ); + PathHelper.fromChoreoPath("AmpLanePABCSprint.1", true, true), + PathHelper.fromChoreoPath("AmpLanePABCSprint.2"), + PathHelper.fromChoreoPath("AmpLanePABCSprint.3"), + PathHelper.fromChoreoPath("AmpLanePABCSprint.4") + ); addCommands( // new ShootSequence(shooter, indexer), - getPath(0).getCommand(), + getPath(0).getCommand(drivetrain), // new ShootSequence(shooter, indexer), - getPath(1).getCommand(), + getPath(1).getCommand(drivetrain), // new ShootSequence(shooter, indexer), - getPath(2).getCommand(), + getPath(2).getCommand(drivetrain), // new ShootSequence(shooter, indexer) - getPath(3).getCommand() + getPath(3).getCommand(drivetrain) ); } } diff --git a/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePHGFSprint.java b/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePHGFSprint.java index 059638e7..b49e5fc3 100644 --- a/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePHGFSprint.java +++ b/src/main/java/org/team1540/robot2024/commands/autos/SourceLanePHGFSprint.java @@ -3,25 +3,25 @@ import edu.wpi.first.wpilibj2.command.PrintCommand; import org.team1540.robot2024.subsystems.drive.Drivetrain; import org.team1540.robot2024.util.AutoCommand; -import org.team1540.robot2024.util.PathPlannerHelper; +import org.team1540.robot2024.util.PathHelper; public class SourceLanePHGFSprint extends AutoCommand { public SourceLanePHGFSprint(Drivetrain drivetrain){ super("SourceLanePHGFSprint"); addPath( - new PathPlannerHelper(drivetrain, "SourceLanePHGFSprint.1", true, true, true), - new PathPlannerHelper(drivetrain, "SourceLanePHGFSprint.2", true), - new PathPlannerHelper(drivetrain, "SourceLanePHGFSprint.3", true), - new PathPlannerHelper(drivetrain, "SourceLanePHGFSprint.4", true) + PathHelper.fromChoreoPath("SourceLanePHGFSprint.1", true, true), + PathHelper.fromChoreoPath("SourceLanePHGFSprint.2"), + PathHelper.fromChoreoPath("SourceLanePHGFSprint.3"), + PathHelper.fromChoreoPath("SourceLanePHGFSprint.4") ); addCommands( - getPath(0).getCommand(), + getPath(0).getCommand(drivetrain), new PrintCommand("Past 1"), - getPath(1).getCommand(), + getPath(1).getCommand(drivetrain), new PrintCommand("Past 2"), - getPath(2).getCommand(), + getPath(2).getCommand(drivetrain), new PrintCommand("Past 3"), - getPath(3).getCommand(), + getPath(3).getCommand(drivetrain), new PrintCommand("Past 4") ); } diff --git a/src/main/java/org/team1540/robot2024/util/AutoCommand.java b/src/main/java/org/team1540/robot2024/util/AutoCommand.java index b653fb62..44772a0e 100644 --- a/src/main/java/org/team1540/robot2024/util/AutoCommand.java +++ b/src/main/java/org/team1540/robot2024/util/AutoCommand.java @@ -1,21 +1,11 @@ package org.team1540.robot2024.util; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.path.PathConstraints; -import com.pathplanner.lib.path.PathPlannerPath; -import com.pathplanner.lib.path.PathPlannerTrajectory; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.trajectory.Trajectory; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.ProxyCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import org.team1540.robot2024.subsystems.drive.Drivetrain; import java.util.ArrayList; import java.util.Arrays; -import java.util.LinkedList; import java.util.List; public class AutoCommand extends SequentialCommandGroup { @@ -23,7 +13,7 @@ public class AutoCommand extends SequentialCommandGroup { private boolean isResetting = false; private Pose2d initialPose = null; - private List paths = new ArrayList<>(); + private List paths = new ArrayList<>(); private int index = 0; @@ -50,22 +40,22 @@ public Pose2d getInitialPose() { return initialPose; } - public void addPath(PathPlannerHelper... paths){ + public void addPath(PathHelper... paths){ this.paths.addAll(Arrays.stream(paths).toList()); - if(this.paths.size() != 0 && this.paths.size() == Arrays.stream(paths).count()){ + if(!this.paths.isEmpty() && this.paths.size() == Arrays.stream(paths).count()){ isResetting = this.paths.get(0).isResetting; initialPose = this.paths.get(0).initialPose; } } - public List getPaths() { + public List getPaths() { return paths; } - public PathPlannerHelper getPath(int index){ + public PathHelper getPath(int index){ return paths.get(index); } - public PathPlannerHelper getNextPath(){ + public PathHelper getNextPath(){ return getPath(index++); } public int getIndex(){ diff --git a/src/main/java/org/team1540/robot2024/util/PathHelper.java b/src/main/java/org/team1540/robot2024/util/PathHelper.java new file mode 100644 index 00000000..234db91c --- /dev/null +++ b/src/main/java/org/team1540/robot2024/util/PathHelper.java @@ -0,0 +1,86 @@ +package org.team1540.robot2024.util; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.util.GeometryUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import org.team1540.robot2024.subsystems.drive.Drivetrain; + +import java.util.function.BooleanSupplier; +import java.util.function.Supplier; + +public class PathHelper { + private static final PathConstraints constraints = new PathConstraints( + 3.0, 4.0, + Units.degreesToRadians(540), Units.degreesToRadians(720) + ); + + boolean isResetting; + Pose2d initialPose; + String pathname; + boolean isChoreo; + boolean canFlip; + PathPlannerPath path; + + + public static PathHelper fromChoreoPath(String pathname) { + return new PathHelper(pathname, true, false, true); + } + + public static PathHelper fromChoreoPath(String pathname, boolean shouldReset, boolean canFlip) { + return new PathHelper(pathname, true, shouldReset, canFlip); + } + + public static PathHelper fromPathPlannerPath(String pathname) { + return new PathHelper(pathname, false, false, true); + } + + public static PathHelper fromPathPlannerPath(String pathname, boolean shouldReset, boolean canFlip) { + return new PathHelper(pathname, false, shouldReset, canFlip); + } + + + private PathHelper(String pathname, boolean isChoreo, boolean shouldReset, boolean canFlip) { + this.pathname = pathname; + this.isChoreo = isChoreo; + this.isResetting = shouldReset; + this.canFlip = canFlip; + this.path = isChoreo ? PathPlannerPath.fromChoreoTrajectory(pathname) : PathPlannerPath.fromPathFile(pathname); + Rotation2d rotation = path.getPoint(0).rotationTarget == null ? new Rotation2d() : path.getPoint(0).rotationTarget.getTarget(); + this.initialPose = new Pose2d(path.getPoint(0).position, rotation); + } + + public boolean getIsResetting() { + return this.isResetting; + } + + public Pose2d getInitialPose() { + return initialPose; + } + + public PathPlannerPath getPath() { + return path; + } + + public Command getCommand(Drivetrain drivetrain, boolean shouldRealign) { + BooleanSupplier shouldFlip = () -> DriverStation.getAlliance().orElse(null) == DriverStation.Alliance.Red; + Supplier flippedInitialPose = () -> shouldFlip.getAsBoolean() ? GeometryUtil.flipFieldPose(initialPose) : initialPose; + Command command = new ConditionalCommand( + AutoBuilder.pathfindThenFollowPath(path, constraints), + AutoBuilder.followPath(path), + () -> drivetrain.getPose().getTranslation().getDistance((flippedInitialPose.get()).getTranslation()) > 1 && shouldRealign); //TODO tune this distance + Command resetCommand = new InstantCommand(() -> drivetrain.setPose(flippedInitialPose.get())); + return isResetting ? resetCommand.andThen(command) : command; + } + + public Command getCommand(Drivetrain drivetrain) { + return getCommand(drivetrain, false); + } +} diff --git a/src/main/java/org/team1540/robot2024/util/PathPlannerHelper.java b/src/main/java/org/team1540/robot2024/util/PathPlannerHelper.java deleted file mode 100644 index 17a290b9..00000000 --- a/src/main/java/org/team1540/robot2024/util/PathPlannerHelper.java +++ /dev/null @@ -1,75 +0,0 @@ -package org.team1540.robot2024.util; - -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.path.PathConstraints; -import com.pathplanner.lib.path.PathPlannerPath; -import com.pathplanner.lib.util.GeometryUtil; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.trajectory.Trajectory; -import edu.wpi.first.math.trajectory.TrajectoryGenerator; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.*; -import org.team1540.robot2024.Constants; -import org.team1540.robot2024.subsystems.drive.Drivetrain; - -import java.util.function.BooleanSupplier; - -public class PathPlannerHelper{ - boolean isResetting = false; - Pose2d initialPose; - Drivetrain drivetrain; - String pathname; - boolean isChoreo; - boolean canFlip; - PathPlannerPath path; - - public PathPlannerHelper(Drivetrain drivetrain, String pathname, boolean isChoreo){ - this(drivetrain, pathname, isChoreo, false, true); - } - - public PathPlannerHelper(Drivetrain drivetrain, String pathname, boolean isChoreo, boolean shouldReset, boolean canFlip){ - this.drivetrain = drivetrain; - this.pathname = pathname; - this.isChoreo = isChoreo; - this.isResetting = shouldReset; - this.canFlip = canFlip; - this.path = isChoreo ? PathPlannerPath.fromChoreoTrajectory(pathname) : PathPlannerPath.fromPathFile(pathname); - Rotation2d rotation = path.getPoint(0).rotationTarget == null ? new Rotation2d() : path.getPoint(0).rotationTarget.getTarget(); - this.initialPose = new Pose2d(path.getPoint(0).position, rotation); - } - - public boolean getIsResetting() { - return this.isResetting; - } - - public Pose2d getInitialPose() { - return initialPose; - } - - public PathPlannerPath getPath(){ - return path; - } - - public Command getCommand(boolean shouldRealign){ - PathConstraints constraints = new PathConstraints( - 3.0, 4.0, - Units.degreesToRadians(540), Units.degreesToRadians(720)); - BooleanSupplier shouldFlip = ()->(DriverStation.getAlliance().orElse(null) == DriverStation.Alliance.Red); - Command command = new ConditionalCommand( - AutoBuilder.pathfindThenFollowPath(path,constraints), - AutoBuilder.followPath(path), - ()->drivetrain.getPose().getTranslation().getDistance((shouldFlip.getAsBoolean() ? GeometryUtil.flipFieldPose(initialPose) : initialPose).getTranslation()) > 1 && shouldRealign); //TODO tune this distance - Command resetCommand = new InstantCommand(() -> drivetrain.setPose(shouldFlip.getAsBoolean() ? GeometryUtil.flipFieldPose(initialPose) : initialPose)); - if(isResetting)return resetCommand.andThen(command); - else return command; - } - public Command getCommand(){ - return getCommand(false); - } -}