targetPos and targetVel shouldnt be global variables in MotionControl.cpp