Added a small servo function. will fix later