Update vel_control.cpp

Final version .. working