add unit tests

[Ishan1522]

TODO:

• Make tests pass
• add vision tests
• add tests for util & helper methods
• Make tests for autonomous?
• add tests for/using sim?
• more advanced unit testing method
• add tests for examples?
• add tests for swerve drive
• add tests for swerve module