-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
refactor: restructure PathPlannerHelper
- Loading branch information
Showing
5 changed files
with
111 additions
and
111 deletions.
There are no files selected for viewing
21 changes: 10 additions & 11 deletions
21
src/main/java/org/team1540/robot2024/commands/autos/AmpLanePABCSprint.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,32 +1,31 @@ | ||
| 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 { | ||
|
|
||
| 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) | ||
| ); | ||
| } | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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<Pose2d> 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); | ||
| } | ||
| } |
75 changes: 0 additions & 75 deletions
75
src/main/java/org/team1540/robot2024/util/PathPlannerHelper.java
This file was deleted.
Oops, something went wrong.