Feature: Center of Mass for Swerve Drive

[sorenfnt]

Get the swerve drive to work with variable center of mass.

The implementation should involve storing the relative positions and distances of each module (by the module's center of rotation) to be passed in during the instantiation of the swerve drive class. The outputs of the swerve math should then be normed so that the drive speed of each module is inversely proportional to it's lever arm (distance to CoM).

The easiest way to do this may be to re-write all of the math, using either the WPILib swerve kinematics, or the general mathematical model for swerve (see https://dominik.win/blog/programming-swerve-drive/ )

Otherwise, assuming the current math works for a square bot, it may work to just scale the drive outputs by 1/(distance from CoM), after norming all of these distances to their average (so that a module which is the average distance away out of the 4 modules is multiplied by 1, those that are closer than average are scaled up, and those that are further than average are scaled down)... this is easier but may be less accurate.

Acceptance Test: The rectangular bot with offset CoM drives well without any significant drift (after any error in measurement has been eliminated)

Preconditions: Square swerve drive is functioning. All swerve modules behave predictably, adjust quickly (rotate PID is working) and the gyroscope is calibrated.

[Alexyglenn]

Simpler version works.

[Alexyglenn]

Done.