You could use the PID library.
http://playground.arduino.cc/Code/PIDLibrary http://playground.arduino.cc/Code/PIDAutotuneLibrary