fixed servo interpolation
fixed set_pos