Add first version of PID controller (not yet done)