Update SN_SuperSwerve to have velocities calculator

We should just pass the axis we want and the code should do all the math we always do.

Any constants we need should be passed tot the SN_SuperSwerve constructor

This function should also deal with field flipping by reading the driver station alliance color