[logicalhippo]

I'm not really familiar with the code that FIRST gave you, but basically what you want to do is:

send your TC command
wait for 1 T packet
if pixels = 0
{
-send a \r
-send the set servo command to the next counter
-repeat from beginning
}
else
{ //(it found a color)
-send a \r
-enable auto-servo mode
-send the TC command again
}

We're not using servos and just rotating the whole robot. The only advantages I can see to the servos are using the tilt angle to calculate distance and pan and scanning before the match begins.

wow.. commented pseudocode...