Finished.

* One bug where servo's won't move to maximum position