Added code to avoid integral windup in PID loop