mini RTOS [sic]

As long as the compiler supports function pointers, something like this could work splendidly, in my opinion.

(I'm at the office now, or I'd check it out myself. I really need to get a job where tinkering with Arduino is acceptable....)