diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 41a6c05..f6686d3 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -89,6 +89,9 @@ public class Drive extends SubsystemBase { }; private ChassisVelocities chassisVelocities; + // PathPlanner trajectory logging (stored each loop for AKit compatibility) + private Pose2d[] lastTrajectory = new Pose2d[0]; + // PID controllers for following Choreo trajectories private final PIDController xController = new PIDController(8.01, 0.0, 0.0); private final PIDController yController = new PIDController(8.01, 0.0, 0.0); @@ -127,12 +130,7 @@ public Drive( Pathfinding.setPathfinder(new LocalADStarAK()); PathPlannerLogging.setLogActivePathCallback( (activePath) -> { - Logger.recordOutput( - "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()])); - }); - PathPlannerLogging.setLogTargetPoseCallback( - (targetPose) -> { - Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose); + if (!activePath.isEmpty()) lastTrajectory = activePath.toArray(new Pose2d[0]); }); // Configure SysId @@ -217,6 +215,9 @@ public void periodic() { gyroDisconnectedAlert.set(gyroDisconnected); Logger.recordOutput("Faults/Drive/GyroDisconnected", gyroDisconnected); + // Log PathPlanner trajectory (stored by callback, recorded here for AKit compatibility) + Logger.recordOutput("Odometry/Trajectory", lastTrajectory); + // Profiling output if (FeatureFlags.PROFILING_ENABLED) { long totalMs = (t6 - startNanos) / 1_000_000;