Finished refactoring.

* Can now calibrate servo's
* Can almost send commands to multiple pololu's