A majority of the code contained in this comes from Swerve Drive Specialties online swerve-lib: https://github.com/SwerveDriveSpecialties/swerve-lib-2022-unmaintained I have just added some things so that I can have access to more variables.
This Library is not made for use by others, and currently does not have the appropriate documentation for what it does.