First working version of PID controller (parameters need tuning)