Pid algorithm, weighted average from sensors