Move i2c reading code into a seperate function