added I2C functions, working on I2C ISR