Added waypoints code with IMU.

Don't work on this code.  It's just here to show you how to integrate
the IMU (compass) into the code.  Compiles fine, but IMU need
calibrating and program needs testing.