Swerve Drive - Reconfigure Swerve Drive math and odometry from speed to position

[Inspirol]

The 2023 WPImath version has swapped the parameters for the swerve drive from speed to angle and position, rendering our swerve drive useless unless we convert the parameters or revert back to WPImath 2022