[Ssbfalcon]

That looks like the answer. Be sure to trace variables used in loops and if's etc if you find that things aren't working the way you want them to.

Also, for autonomous mode testing and such, having a mock competition port is very useful. Firstly so you can activate autonomous mode with the flick of a switch, and also if that goes crazy, you can disable it by pressing the disable button.