Sounds like a very interesting project. I am currently investigating options for a light weight RTOS for my robot. Most of what i have seen is loaded with many functions I do not need nor want. Since there is limited resources on thease parts having just what you need is a plus. unfortunatly, the link to your code is not functoning. Have you uploaded the file in another place?