added flag control to drive-only_teleop.c, put function in TeleOp_functions.h