Make sure semaphores are safe in ISRs. Probably need to make semaphore storage and associated macros volatile