Once you have it in a loop, you'd probably do much better not to be constantly resetting the PID parameters while it's enabled. I'd try something more like:
Code:
switch (step){
case 1:
pid.SetTolerance(.1);
pid.SetOutputRange(-.5, .5);
pid.SetSetpoint(497);
pid.Enable();
step++;
break;
case 2:
if(pid.OnTarget()){
step++;
}
break;
case 3:
pid.Disable();
pid.SetTolerance(.1);
pid.SetOutputRange(-.25, .25);
pid.SetSetpoint(800);
pid.Enable();
step++;
break;
case 4:
if(pid.OnTarget()){
step++;
}
break;
default:
break;
}
Or, for brevity at the cost of readability, you could remove the case 4 block and move the case 4 statement up next to the case 2 statement, or remove case 4 entirely and never disable the PID in autonomous if you want to leave it on until the end; in this case, you should put the disable outside the loop.