Added a feedforward term for velocity commands in PID loop