Made the follow path algorithm work