Add PID controller for velocity