add simple PID to controll