Fix I2C write function