Add PID controller

the parameters need to optimize