Finished the lab, made almost everything work. Just need to make the check if the start position is on an obstacle