Simple servo conrol

What changed
============

Able to control a servo motor

What's next
=========

* Testing
* Code optimizing