Add Swerve Drive Support

[danielbrownmsm]

Add Swerve Drive Support

Summary We have a swerve drive. It needs to be coded.

Work Required

• code the swerve drive
• test the swerve drive

Verification

• CI passes

[jkleiber]

We should make the swerve drive into its own subsystem rather than mixing it with the Drivetrain/DrivetrainIO system. Something like SwerveDrive and SwerveDriveIO seem like names we could use

[danielbrownmsm]

why?

[danielbrownmsm]

also, research thus far:

• https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/swerve-drive-kinematics.html
• https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/swerve-drive-odometry.html
• https://github.wpilib.org/allwpilib/docs/beta/cpp/classfrc_1_1_swerve_drive_kinematics.html WPILib has a surprising amount of swerve drive support

[jkleiber]

It makes more sense to start with a clean slate since the swerve drive will use different amounts of motors and different driving/odometry logic. It's almost totally different so we should make it a separate subsystem, saving the current drivetrain code as legacy code in case we make a west coast drive again

[danielbrownmsm]

Working under the assumption that we are plugging the absolute encoders into the roboRIO. If we're plugging them directly into the sparkMAXes instead, we'll need to change some code

[danielbrownmsm]

https://github.com/wpilibsuite/allwpilib/blob/main/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp