Pid Control Loop Algorithm