You can get the latest code from here:
https://github.com/faust1706/Smooth-Swerve
The simulation is what this code displays as of right now, but that can be changed. To alter the simulation, change the waypoints to whatever and however many you want. Make sure the heading variable is of the same size as the waypoints array size, otherwise the program will get an outofbounds error.
I have not rigorously tested this code, so feel free to try to break it. If you do break it, either by making it generate an incorrect path or something, then leave a comment here or make it an issue on the repo's page.
The simulation is also completely smooth now thanks to the programmer on 2046.