started on autons

Author: PotatoBoyH4

src/main/java/frc/robot/commands/auton/AutoCommandInterface.java

@@ -0,0 +1,5 @@
+package frc.robot.commands.auton;
+
+public interface AutoCommandInterface {
+  public void reassignAlliance();
+}

src/main/java/frc/robot/commands/drive/DriveAutonCommand.java

@@ -0,0 +1,102 @@
+package frc.robot.commands.drive;
+
+import choreo.Choreo;
+import choreo.trajectory.SwerveSample;
+import choreo.trajectory.Trajectory;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.commands.auton.AutoCommandInterface;
+import frc.robot.subsystems.drive.DriveSubsystem;
+import java.util.Optional;
+import org.slf4j.Logger;
+import org.slf4j.LoggerFactory;
+
+public class DriveAutonCommand extends Command implements AutoCommandInterface {
+  private final DriveSubsystem driveSubsystem;
+  private Trajectory<SwerveSample> trajectory;
+  private final Timer timer = new Timer();
+  private static final Logger logger = LoggerFactory.getLogger(DriveAutonCommand.class);
+  private boolean isTherePath = false;
+  private String trajectoryName;
+  private boolean mirrorTrajectory = false;
+
+  private boolean resetOdometry;
+  private boolean lastPath;
+
+  public DriveAutonCommand(
+      DriveSubsystem driveSubsystem,
+      String trajectoryName,
+      boolean lastPath,
+      boolean resetOdometry) {
+
+    addRequirements(driveSubsystem);
+    this.driveSubsystem = driveSubsystem;
+    this.resetOdometry = resetOdometry;
+    this.lastPath = lastPath;
+    this.trajectoryName = trajectoryName;
+    Optional<Trajectory<SwerveSample>> tempTrajectory = Choreo.loadTrajectory(trajectoryName);
+    if (tempTrajectory.isPresent()) {
+      trajectory = tempTrajectory.get();
+      isTherePath = true;
+    } else {
+      logger.error("Trajectory {} not found", trajectoryName);
+      isTherePath = false;
+    }
+    timer.start();
+  }
+
+  @Override
+  public void reassignAlliance() {
+    mirrorTrajectory = driveSubsystem.shouldFlip();
+  }
+
+  @Override
+  public void initialize() {
+    Pose2d initialPose = new Pose2d();
+    if (trajectory.getInitialPose(mirrorTrajectory) != null) {
+      initialPose = trajectory.getInitialPose(mirrorTrajectory).get();
+    }
+    if (resetOdometry && trajectory.getInitialPose(mirrorTrajectory) != null) {
+      driveSubsystem.resetOdometry(initialPose);
+    }
+    driveSubsystem.setEnableHolo(true);
+    // driveSubsystem.recordAutoTrajectory(trajectory);
+    driveSubsystem.resetHolonomicController();
+    driveSubsystem.grapherTrajectoryActive(true);
+    timer.reset();
+    logger.info("Begin Trajectory: {}", trajectoryName);
+    SwerveSample desiredState = trajectory.sampleAt(timer.get(), mirrorTrajectory).get();
+    driveSubsystem.calculateController(desiredState);
+  }
+
+  @Override
+  public void execute() {
+    SwerveSample desiredState = trajectory.sampleAt(timer.get(), mirrorTrajectory).get();
+    driveSubsystem.calculateController(desiredState);
+  }
+
+  @Override
+  public boolean isFinished() {
+    if (isTherePath) {
+      return false;
+    }
+    return timer.hasElapsed(trajectory.getTotalTime());
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    driveSubsystem.setEnableHolo(false);
+    // driveSubsystem.recordAutoTrajectory(null);
+
+    if (!interrupted && !lastPath) {
+      driveSubsystem.calculateController(
+          trajectory.sampleAt(trajectory.getTotalTime(), mirrorTrajectory).get());
+    } else {
+      driveSubsystem.drive(0, 0, 0);
+    }
+
+    driveSubsystem.grapherTrajectoryActive(false);
+    logger.info("End Trajectory {}: {}", trajectoryName, timer.get());
+  }
+}

src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java

@@ -1,3 +1,38 @@
 package frc.robot.subsystems.pathHandler;
 
-public class PathHandler {}
+import choreo.trajectory.SwerveSample;
+import choreo.trajectory.Trajectory;
+import edu.wpi.first.wpilibj.Timer;
+import frc.robot.subsystems.drive.DriveSubsystem;
+import java.util.List;
+import java.util.Set;
+import org.strykeforce.telemetry.measurable.MeasurableSubsystem;
+import org.strykeforce.telemetry.measurable.Measure;
+
+public class PathHandler extends MeasurableSubsystem {
+  DriveSubsystem driveSubsystem;
+
+  private PathStates currStates = PathStates.DONE;
+  private List<String> NodeNames;
+  private Timer timer = new Timer();
+  private Trajectory<SwerveSample>[][] paths;
+  private String[][] pathNames;
+
+  PathHandler(DriveSubsystem driveSubsystem) {
+    this.driveSubsystem = driveSubsystem;
+  }
+
+  @Override
+  public Set<Measure> getMeasures() {
+    // TODO Auto-generated method stub
+    throw new UnsupportedOperationException("Unimplemented method 'getMeasures'");
+  }
+
+  public enum PathStates {
+    FETCH,
+    DRIVE_FETCH,
+    PLACE,
+    DRIVE_PLACE,
+    DONE
+  }
+}