Add Pong

Update Waypoint Control code. Add beginnings of Pong.