Stubbed out auto path logging

src/main/java/com/team1701/lib/util/GeometryUtil.java

@@ -21,7 +21,16 @@ public final class GeometryUtil {
     public static final Rotation3d kRotation3dIdentity = new Rotation3d();
 
     public static Rotation2d flipX(Rotation2d rotation) {
-        return rotation.getRadians() < 0 ? kRotationMinusPi.minus(rotation) : kRotationPi.minus(rotation);
+        return kRotationPi.minus(rotation);
+    }
+
+    public static Pose2d flipX(Pose2d pose, double fieldLength) {
+        return new Pose2d(fieldLength - pose.getX(), pose.getY(), flipX(pose.getRotation()));
+    }
+
+    public static Pose2d[] flipX(Pose2d[] poses, double fieldLength) {
+        // TODO: Flip poses
+        return poses;
     }
 
     public static Rotation2d angleModulus(Rotation2d rotation) {

src/main/java/com/team1701/lib/util/PathBuilder.java

@@ -0,0 +1,41 @@
+package com.team1701.lib.util;
+
+import java.util.ArrayList;
+import java.util.Collections;
+import java.util.List;
+
+import com.pathplanner.lib.path.PathPlannerPath;
+import edu.wpi.first.math.geometry.Pose2d;
+
+public class PathBuilder {
+    private final List<Pose2d> mPath = new ArrayList<>();
+
+    public Pose2d[] build() {
+        return mPath.toArray(new Pose2d[mPath.size()]);
+    }
+
+    public Pose2d[] buildAndClear() {
+        var path = build();
+        clear();
+        return path;
+    }
+
+    public void clear() {
+        mPath.clear();
+    }
+
+    public PathBuilder addPose(Pose2d pose) {
+        // TODO: Add to mPath
+        return this;
+    }
+
+    public PathBuilder addPath(Pose2d[] path) {
+        Collections.addAll(mPath, path);
+        return this;
+    }
+
+    public PathBuilder addPath(PathPlannerPath path) {
+        // TODO: Add to mPath
+        return this;
+    }
+}

src/main/java/com/team1701/robot/RobotContainer.java

@@ -170,10 +170,37 @@ private void setupAutonomous() {
         PathPlannerLogging.setLogActivePathCallback(
                 poses -> Logger.recordOutput("PathPlanner/Path", poses.toArray(Pose2d[]::new)));
 
+        // TODO: Create wrapper class for autonomous chooser
+        // TODO: Also update on change to team color
         var commands = new AutonomousCommands(mRobotState, mDrive);
-        autonomousModeChooser.addDefaultOption("Demo", commands.demo());
-        autonomousModeChooser.addOption("Four Piece", commands.fourPiece());
+        var demoCommand = commands.demo();
+        var fourPieceCommand = commands.fourPiece();
+
+        autonomousModeChooser.addDefaultOption("Demo", demoCommand.command());
+        autonomousModeChooser.addOption("Four Piece", fourPieceCommand.command());
         autonomousModeChooser.addOption("Four Piece Planner", new PathPlannerAuto("FourPiece"));
+
+        autonomousModeChooser.getSendableChooser().onChange(name -> {
+            Pose2d[] path;
+            switch (name) {
+                case "Demo":
+                    path = demoCommand.path();
+                    break;
+                case "Four Piece":
+                    path = fourPieceCommand.path();
+                    break;
+                default:
+                    path = new Pose2d[] {};
+                    break;
+            }
+
+            if (Configuration.isRedAlliance()) {
+                path = GeometryUtil.flipX(path, FieldConstants.kFieldLongLengthMeters);
+            }
+
+            // TODO: Also export as Translation2d[]
+            Logger.recordOutput("Autonomous/PathPose2d", path);
+        });
     }
 
     private void setupStateTriggers() {

src/main/java/com/team1701/robot/commands/AutonomousCommands.java

@@ -8,6 +8,7 @@
 import com.pathplanner.lib.path.PathPlannerPath;
 import com.team1701.lib.swerve.SwerveSetpointGenerator.KinematicLimits;
 import com.team1701.lib.util.GeometryUtil;
+import com.team1701.lib.util.PathBuilder;
 import com.team1701.robot.Configuration;
 import com.team1701.robot.Constants;
 import com.team1701.robot.FieldConstants;
@@ -23,6 +24,13 @@
 public class AutonomousCommands {
     private final RobotState mRobotState;
     private final Drive mDrive;
+    private PathBuilder mPathBuilder = new PathBuilder();
+
+    public static record AutonomousCommand(Command command, Pose2d[] path) {
+        public AutonomousCommand(Command command) {
+            this(command, new Pose2d[] {});
+        }
+    }
 
     public AutonomousCommands(RobotState robotState, Drive drive) {
         mRobotState = robotState;
@@ -32,10 +40,7 @@ public AutonomousCommands(RobotState robotState, Drive drive) {
     }
 
     private Pose2d autoFlipPose(Pose2d pose) {
-        var flippedPose = new Pose2d(
-                FieldConstants.kFieldLongLengthMeters - pose.getX(),
-                pose.getY(),
-                GeometryUtil.kRotationPi.minus(pose.getRotation()));
+        var flippedPose = GeometryUtil.flipX(pose, FieldConstants.kFieldLongLengthMeters);
         return Configuration.isRedAlliance() ? flippedPose : pose;
     }
 
@@ -93,11 +98,13 @@ private Command followChoreoPath(String pathName, boolean resetPose) {
             return idle(); // Prevent auto mode from continuing if trajectory failed to load
         }
 
+        mPathBuilder.addPath(trajectory.getPoses());
+
         return new DriveChoreoTrajectory(mDrive, trajectory, mRobotState, resetPose);
     }
 
-    public Command demo() {
-        return loggedSequence(
+    public AutonomousCommand demo() {
+        var command = loggedSequence(
                         print("Starting demo"),
                         followPath("demo1", true),
                         driveToPose(new Pose2d(2.0, 1.0, Rotation2d.fromRadians(-Math.PI * 2.0 / 3.0))),
@@ -108,13 +115,17 @@ public Command demo() {
                         followPath("demo2"),
                         driveToPose(new Pose2d(10.0, 5.0, GeometryUtil.kRotationMinusHalfPi), false))
                 .withName("AutonomousDemo");
+
+        return new AutonomousCommand(command, new Pose2d[] {}); // TODO: Add path
     }
 
-    public Command fourPiece() {
-        return loggedSequence(
+    public AutonomousCommand fourPiece() {
+        var command = loggedSequence(
                 print("Started four piece auto"),
                 followChoreoPath("FourPiece.1", true),
                 followChoreoPath("FourPiece.2"),
                 followChoreoPath("FourPiece.3"));
+
+        return new AutonomousCommand(command, mPathBuilder.buildAndClear());
     }
 }