Fix Swerve Drive Path Planner

[abuddies0]

useful docs: https://github.com/mjansen4857/pathplanner/wiki/Java-Example:-Build-an-Auto
useful chief delphi post: https://www.chiefdelphi.com/t/configuring-autobuilder-for-frc-pathplanner-what-is-a-ramsete-ltv-holonomic-controller/443028

[abuddies0]

The coordinate system is setup. PathPlanner is getting the current positions and speeds (I checked with PathPlannerLogger). The command is scheduled and ran as seen by how it accelerates. The command never ends even after reaching the desired goal and I don't know why. In fact, it keeps speeding up for all of eternity for no reason.