Added ISR routine

Added a ISR to better handle the button presses to switch modes.
Cleaned up other aspects of the code.  Still have some debugging to
do...