Starting controller work. Waypoints
Tested waypoints