Improved I2C function

Taken from my Balancing robot:
https://github.com/TKJElectronics/Balanduino/blob/master/Firmware/Baland
uino/I2C.ino