Servo init functions

update
bug fix
warnings clean