Complete the new I2C operating function with timeout mechanism
Create mpu6050 read/write function