working servo code now