Fix servo code