You can use a state machine. We posted our code on line. Take a look at what we did. It will help you a lot for next year if you do. Our autonomous code look very simple and is very easy to edit. You can stop and start it with out having to reboot.
Look up team 107 code post
http://www.chiefdelphi.com/forums/sh...d.php?t=137046
If you have any questions let me know.