[Drive] Add 254's Swerve Setpoint generator

[GreenTomato5]

Take PP implementation or just take it from 254's 2024 code & test in sim (the second is probably a better idea).

[GreenTomato5]

Whoever does this should read the docs from path planner then read 254 code and add the 254 code. 254 Also uses ctre swerve with basically the same IO layer structure (outside of that its mad different tho) so it should be easy to take. Here's their code : https://github.com/Team254/FRC-2024-Public

[GreenTomato5]

Here's how you set the control request (I started on this earlier today 💀 ). U just have to find a way to cache past module state and delta t that's accurate. (254 knows how)
private final SwerveSetpointGenerator setpointGenerator = new SwerveSetpointGenerator(ROBOT_CONFIG, ANGULAR_VELOCITY_LIMIT);

public void driveFieldRelSetpointGen(ChassisSpeeds targetSpeeds, SwerveSetpoint moduleStates) {
    driveIO.getDrive().setControl(new SwerveRequest.ApplyRobotSpeeds().withSpeeds(setpointGenerator.generateSetpoint(moduleStates, targetSpeeds, 1).robotRelativeSpeeds()));
}