Updated to use TIMER16[0] or systick