Added teleop.c and updated teleopFunctions.h