[davidthefat]

Eh, not exactly "custom" function... Personally, I feel that there is no point in using C++ if you are not going to use classes per se. I think it defeats the whole purpose of using an object oriented language. Yes, I know the library is essentially a whole bunch of classes and straight C would not recognize it. Personally, I would say clean up the code. I am not saying it is not "dirty" (it can be optimized...) but it is congested. I learned the very hard way, once when a fairly good programmer gave me a lecture on not splitting up the source into separate files and classes and other last year, when I essentially wrote spaghetti code, the inspector and mentor was appalled by it. Robotics does tend to make you write any code that "works" over organized, optimized and clean code that works.

Ok, see what I wrote before (it's not going to run as is, it is essentially pseudo code) and pay attention to why I do certain things. I will not spoon feed you.