Skip to content

Commit

Permalink
feat: add AutoCommand to git
Browse files Browse the repository at this point in the history
  • Loading branch information
WeilSimon committed Feb 6, 2024
1 parent 4ac895b commit b6c63d6
Show file tree
Hide file tree
Showing 2 changed files with 80 additions and 3 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -8,10 +8,10 @@
import org.team1540.robot2024.util.AutoManager;
import org.team1540.robot2024.util.PathPlannerHelper;

public class ThreeCloseTop extends AutoCommand {
public class AmpSide3Close extends AutoCommand {

ThreeCloseTop(Drivetrain drivetrain, Shooter shooter, Indexer indexer){
super("3CloseTop");
AmpSide3Close(Drivetrain drivetrain, Shooter shooter, Indexer indexer){
super("AmpSide3Close");

addPath(
new PathPlannerHelper(drivetrain, "3CloseTop.1", true),
Expand Down
77 changes: 77 additions & 0 deletions src/main/java/org/team1540/robot2024/util/AutoCommand.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,77 @@
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 int index = 0;

public AutoCommand(String name){
this.name = name;
}

public AutoCommand(String name, Command command){
this(name);
addCommands(command);
}



public void setName(String name) {
this.name = name;
}

public String getName() { return name;}
public boolean getIsResetting() {
return this.isResetting;
}
public Pose2d getInitialPose() {
return initialPose;
}

public void addPath(PathPlannerHelper... paths){
this.paths.addAll(Arrays.stream(paths).toList());
if(this.paths.size() != 0 && this.paths.size() == Arrays.stream(paths).count()){
isResetting = this.paths.get(0).isResetting;
initialPose = this.paths.get(0).initialPose;
}
}

public List<PathPlannerHelper> getPaths() {
return paths;
}

public PathPlannerHelper getPath(int index){
return paths.get(index);
}
public PathPlannerHelper getNextPath(){
return getPath(index += 1);
}
public int getIndex(){
return index;
}
public void setIndex(int index){
this.index = index;
}
}

0 comments on commit b6c63d6

Please sign in to comment.