better PID anti-saturation logic