first implementation of full PID