What you could do(for autononmous) was have a start time and an end time to all things. Sort of like this:
Code:
//Autonomous code
go(1,0,18); //Go forward (1 = fwd, 2 = rev), start at 0, and end at 1800ms
turn(1,19,25); //Turn (1 = left, 2 = right), start at 1900ms end at 2500ms
//Notice the 1000ms delay!
moveArm(2,35,40); //Move arm ( 1 = up, 2 = down), start at 3500ms end at 4000ms
Of course, you'll need to use interrupts (use the white paper from Innovation First), and add a static counter to the t100msDelay, for increments of 100ms. More precision that using seconds. But, the way you should do it is it just sets a pwm (or whatever) equal to the correct value, and move on, when it comes back (you'll have to make it know that it already completed one and it's time to finish up) just set it to 127 or whatever. Very useful and very effective.