Edits to IMU self test program, still doesnt work...