Add I2CLib and functions for MPU-6050