ITerm windup prevention and immediate Ki evaluation

(1) Avoid integral accumulation when clamping at output=1.
(2) Immediately take into account integral error, instead of at next iteration