Implemented interrupt, not tested.

Program is running at 530Hz.