Quick ISR rewrite

Not tested. Left old test file as oldmain.c, replaced the main,c with a
new one with ISR's in it.