tested on 16mhz
simplifying PID
simplifying PID