Add angular control to servo
Add support of single busy-waiting measurment and triggering signle measurment via interupt