Some refactoring and changed waypts to cycle