Update and rename MAINCODE.ino to Razor_AHRS.ino

Change back to original AHRS code from Razor_AHRS IMU github.
Add debugging println() information.