There are a lot of
while statements in that code that don't look at all right to me. For example,
Code:
while (PAN_SERVO > 130)
{
pwm01 = 147;
pwm02 = 147;
pwm03 = 147;
pwm04 = 147;
}
looks like it will get stuck forever if the PAN_SERVO is greater than 130. Nothing is done inside the loop to let the value of PAN_SERVO change. Take a look at your RC and see if the CODE ERROR light is turning red when you try to run your autonomous routine.
And then try changing those
while statments into
if statements, which is what it looks like they really ought to be.