SwerveModuleState and Odometry

[Nadezaxer]

Functions for getting the SwerveModuleState of the module would be nice to have.

Also functions for odomety in the swerve drive would nice to have with the Gyro being passed in as a suppler, or the kinematics need to be exposed some how to allow for odometry.

[ItsMajestiX]

I worked on these issues in the last few commits and I now have solutions for them:

• SwerveDrive now takes an object that implements Gyro as an argument in its constructor and uses it to update odometry. The odometery can be accessed with SwerveDrive.getPose2d() and SwerveDrive.resetPosition(Pose2d pose)
• The states of the SwerveDriveModules that were used to construct the SwerveDrive object can be accessed using the SwerveDrive.getModuleData(int index) method.

If you want to take a look at these new features they should be in the latest commit to the swerve branch.