Coded the write register function for I2C