diff/pid controller rewritten