Update vel_control.cpp