Reverted changes to original code. However, checking return values for
read out and write to i2c now.