Add unit tests for each component

[colemacphail]

Context

To start, just figure out how to run unit tests on robot code and make unit tests for our drive code since we'll be using a swerve drive for every robot. Future issues can add tests for other subsystems once we have a template for how to set them up.

Focus on testing state transitions. Behaviour in each state can change and is self-described in the state itself so unit tests are likely to change and not really prove anything. On the other hand, state transitions can be complex. Try to think of edge cases where possible. Feel free to whiteboard a state transition diagram if you're interested. Whatever helps :)

Time Estimate: Unknown