feat: add AutoCommand to git

src/main/java/org/team1540/robot2024/commands/autos/ThreeCloseTop.java → src/main/java/org/team1540/robot2024/commands/autos/AmpSide3Close.java

@@ -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),

src/main/java/org/team1540/robot2024/util/AutoCommand.java

@@ -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;
+    }
+}