Added servo controll. This will probably be changed in future.