rearrange servo code