Added interrupt from STC functionality.

Also added wait for TWI and some delays
which need to be checked sometime.