servo class, need testing (even compiling)
fixed a lot of bugs, even working!
Merge branch 'servo' (works!)