Added more robot functionality and tests