Refactored servo control to Legs