Updated code to implement a blocking i2c code for master... works much better