Add PID controller implementation