Fixed bug in MPU6050 test sketch