You probably want to post that on FIRST Codex, cause then it'll be available for all teams to use and see, since FIRST Codex is trying to put together resources for teams for help on programming. By the way, VERY nice code! I'm impressed. Keep up the great work.
http://www.first-codex.net/