Clean up debug logging in herkulex, add error text to maestro