Clean up and improve robot functionality