Refactored I2C Stuff. Made own lib now.