Moved code into the .h/.cpp; ISR still not working