[WarehouseHW] Robot: update algorithm and error handling