Updated with soft i2c code