Added a custom wait function to enable interrupts

Fork of m3pi by Chris Styles