All this servo code can be removed from the repo