Programming "sleep" function

[Bongle]

Declare a global int counter, or a static int counter.

Suppose your autonomous mode can be divided into stages, like:
Stage 1: Drive closer to rack
Stage 2: Use camera to detect rack
Stage 3: Drive to vision target
Stage 4: Position Arm
Stage 5: Place ringer

Inside your autonomous routine, have something like this:

if(counter >= 0 && counter < STAGE_ONE_END)
{ // actions to do in stage 1
}
else if(counter >= STAGE_ONE_END && counter < STAGE_TWO_END)
{ // actions to do in stage 2
}
else if(counter >= STAGE_TWO_END && counter < STAGE_THREE_END)
{ // actions to do in stage 3
}
... // more if statements

counter++;

The actual values of STAGE_ONE_END and so on would be determined experimentally. Or you could decide for exactly how long you want each stage to run, then take the cycle rate of the CPU (xx.x milliseconds) and determine how many iterations are equivalent to your desired time.