Skip to content

Commit

Permalink
refactor: restructure PathPlannerHelper
Browse files Browse the repository at this point in the history
  • Loading branch information
rutmanz committed Feb 11, 2024
1 parent a3872ba commit e31f36b
Show file tree
Hide file tree
Showing 5 changed files with 111 additions and 111 deletions.
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)
);
}
}
Original file line number Diff line number Diff line change
Expand Up @@ -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")
);
}
Expand Down
22 changes: 6 additions & 16 deletions src/main/java/org/team1540/robot2024/util/AutoCommand.java
Original file line number Diff line number Diff line change
@@ -1,29 +1,19 @@
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 {
private String name;
private boolean isResetting = false;
private Pose2d initialPose = null;

private List<PathPlannerHelper> paths = new ArrayList<>();
private List<PathHelper> paths = new ArrayList<>();

private int index = 0;

Expand All @@ -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<PathPlannerHelper> getPaths() {
public List<PathHelper> 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(){
Expand Down
86 changes: 86 additions & 0 deletions src/main/java/org/team1540/robot2024/util/PathHelper.java
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 src/main/java/org/team1540/robot2024/util/PathPlannerHelper.java

This file was deleted.

0 comments on commit e31f36b

Please sign in to comment.