Kiinduláshoz talán elegendő ez:

http://playground.arduino.cc/Code/PIDLibrary