Wrote a test program for the IMU