PID algorithm implementation code, including incremental and positional PID code