Got I2C code working, but its a bit sketchy at the moment, ok for machine.