Added STOP condition wait loop.
Removed debug prinf's.
Changed twi clk speed to 400Khz.