Cleaned up tacho ISR handler - much easier to understand now, and we can get tacho and direction now. Still need to add 2msec callback to output port driver to allow speed to be updated