working servo motor code