Fairly certain I2C is working. Think there might be a logic problem