Clean up servo code