Add servo code