Fixed cavern finding, need to fix robot communication and best path storage