Fix twitching servo, simplify and cleanup