Refactor of updatePID, still needs testing