added face-to-target and follow path for auto

src/main/deploy/pathplanner/paths/Test Face To Target.path

@@ -0,0 +1,102 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.0,
+        "y": 7.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.729224912779601,
+        "y": 7.1380952251821475
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.9690092262746814,
+        "y": 7.0
+      },
+      "prevControl": {
+        "x": 5.123454250215369,
+        "y": 7.523330945456956
+      },
+      "nextControl": {
+        "x": 4.647648868251474,
+        "y": 5.911082949093538
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.5995945633646906,
+        "y": 5.1222893430365755
+      },
+      "prevControl": {
+        "x": 1.278340403541586,
+        "y": 7.138159195926557
+      },
+      "nextControl": {
+        "x": 1.989122270059487,
+        "y": 2.6780029835267265
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.362207436158645,
+        "y": 6.85568763782842
+      },
+      "prevControl": {
+        "x": 2.63748883872656,
+        "y": 6.695486684711854
+      },
+      "nextControl": {
+        "x": 5.212464042958928,
+        "y": 7.264691729857955
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.2782342053414837,
+        "y": 3.651822250263718
+      },
+      "prevControl": {
+        "x": 2.398126362089024,
+        "y": 4.7327616363417775
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 12.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -90.07812463521357
+  },
+  "useDefaultConstraints": false
+}

src/main/java/frc/robot/RobotContainer.java

@@ -237,6 +237,7 @@ private LoggedDashboardChooser<Auto> buildAutoChooser() {
                 "Example Pathplanner Auto",
                 new PathPlannerAutoWrapper("Example Auto")
         );
+        autoSendableChooser.addOption("Example Face To Target", new ExampleFaceToTarget());
         // TODO: add your autos here
 
         SmartDashboard.putData("Select Auto", autoSendableChooser.getSendableChooser());

src/main/java/frc/robot/autos/ExampleFaceToTarget.java

@@ -0,0 +1,29 @@
+package frc.robot.autos;
+
+import com.pathplanner.lib.path.PathPlannerPath;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.RobotContainer;
+import frc.robot.commands.drive.FollowPathFaceToTarget;
+import org.ironmaple.utils.FieldMirroringUtils;
+import org.json.simple.parser.ParseException;
+
+import java.io.IOException;
+
+public class ExampleFaceToTarget implements Auto {
+    @Override
+    public Command getAutoCommand(RobotContainer robot) throws IOException, ParseException {
+        return FollowPathFaceToTarget.followPathFacetToTarget(
+                PathPlannerPath.fromPathFile("Test Face To Target"),
+                0,
+                FieldMirroringUtils.SPEAKER_POSITION_SUPPLIER,
+                null
+        );
+    }
+
+    @Override
+    public Pose2d getStartingPoseAtBlueAlliance() {
+        return new Pose2d(2, 7, Rotation2d.fromDegrees(-90));
+    }
+}

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

@@ -1,41 +1,39 @@
 package frc.robot.commands.drive;
 
 import com.pathplanner.lib.auto.AutoBuilder;
-import com.pathplanner.lib.controllers.PPHolonomicDriveController;
 import com.pathplanner.lib.path.PathPlannerPath;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.subsystems.drive.HolonomicDriveSubsystem;
+import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.subsystems.drive.SwerveDrive;
+import frc.robot.utils.ChassisHeadingController;
 import frc.robot.utils.MapleShooterOptimization;
 
-import java.util.Optional;
 import java.util.function.Supplier;
 
 public class FollowPathFaceToTarget {
-    public static Command followPathFacetToTarget(Supplier<Translation2d> targetPositionSupplier, MapleShooterOptimization shooterOptimization, HolonomicDriveSubsystem driveSubsystem, PathPlannerPath path) {
-        final Supplier<Rotation2d> rotationTargetOverride = () ->
-                shooterOptimization.getShooterFacing(
-                        targetPositionSupplier.get(),
-                        driveSubsystem.getPose().getTranslation(),
-                        driveSubsystem.getMeasuredChassisSpeedsFieldRelative()
-                );
-        return followPathFacetToTarget(driveSubsystem, path, rotationTargetOverride);
+    public static Command followPathFacetToTarget(PathPlannerPath path, double offSetSeconds, Supplier<Translation2d> targetPositionSupplier, MapleShooterOptimization shooterOptimization) {
+        final Runnable requestFaceToTarget = () -> SwerveDrive.swerveHeadingController.setHeadingRequest(
+                new ChassisHeadingController.FaceToTargetRequest(targetPositionSupplier, shooterOptimization)
+        );
+        final Runnable requestNull = () -> SwerveDrive.swerveHeadingController.setHeadingRequest(
+                new ChassisHeadingController.NullRequest()
+        );
+        return AutoBuilder.followPath(path)
+                .deadlineFor(Commands.waitSeconds(offSetSeconds).andThen(requestFaceToTarget))
+                .finallyDo(requestNull);
     }
 
-    public static Command followPathFacetToTarget(Supplier<Translation2d> targetPositionSupplier, HolonomicDriveSubsystem driveSubsystem, PathPlannerPath path) {
-        final Supplier<Rotation2d> rotationTargetOverride = () ->
-                targetPositionSupplier.get().minus(driveSubsystem.getPose().getTranslation()).getAngle();
-        return followPathFacetToTarget(driveSubsystem, path, rotationTargetOverride);
-    }
-
-    public static Command followPathFacetToTarget(HolonomicDriveSubsystem driveSubsystem, PathPlannerPath path, Supplier<Rotation2d> rotationTargetOverride) {
-        final Command followPath = AutoBuilder.followPath(path);
-        followPath.addRequirements(driveSubsystem);
-        return followPath.beforeStarting(
-                        () -> PPHolonomicDriveController.setRotationTargetOverride(
-                                () -> Optional.of(rotationTargetOverride.get()))
-                )
-                .finallyDo(() -> PPHolonomicDriveController.setRotationTargetOverride(Optional::empty));
+    public static Command followPathFacetToTarget(PathPlannerPath path, double offSetSeconds, Supplier<Rotation2d> rotationTargetOverride) {
+        final Runnable requestFaceToRotation = () -> SwerveDrive.swerveHeadingController.setHeadingRequest(
+                new ChassisHeadingController.FaceToRotationRequest(rotationTargetOverride.get())
+        );
+        final Runnable requestNull = () -> SwerveDrive.swerveHeadingController.setHeadingRequest(
+                new ChassisHeadingController.NullRequest()
+        );
+        return AutoBuilder.followPath(path)
+                .deadlineFor(Commands.waitSeconds(offSetSeconds).andThen(requestFaceToRotation))
+                .finallyDo(requestNull);
     }
 }