Update 5.6

Started implementing Waypoints but trying Smoothing first.