Few bugs fixed

Tested with Arduino Due and Mini Pro 3.3v 8Mhz (Timer 1)