Added waypoints code

- Not finished yet