Fixed problem with I2C communication. Was because of address used by initial testing program.