Added beginnings of IMU self-test proram