Refactored and cleaned robot code