Fixed the I2C code, still more work to do.