def DriverRoboDK():
  SERVER_IP = "192.168.0.101"    # IP address of the computer(not the robot!)
  SERVER_PORT = 30000            # Server port on the computer
  # MoveJ speed params
  global accel_radss = 3.0
  global speed_rads  = 0.200 #0.75 default
  # MoveL speed params
  global accel_mss   = 1.2
  global speed_ms    = 0.100 #0.3 default
  # default blend radius (rounding/smoothing)
  global blend_radius = 0.0
  #--------------------------------------------
  MSG_SPEED = 2
  MSG_SPEED4 = 3
  MSG_MOVEJ = 4
  MSG_MOVEL = 5
  MSG_MOVEC = 6
  MSG_SETTOOL = 9
  MSG_CJNT = 10
  MSG_MOVEL_SEARCH = 11
  MSG_MOVEJ_POSE = 13
  MSG_MOVEL_POSE = 14
  MSG_MOVEP = 15
  MSG_MOVEP_POSE = 16
  MSG_MOVEC_POSE = 17
  MSG_POPUP = 20
  MSG_PAUSE = 21
  MSG_RUNPROG = 22 
  MSG_SETDO = 23
  MSG_WAITDI = 24
  MSG_ROUNDING = 25
  MSG_SETAO = 26
  MSG_GETDI = 27
  MSG_GETAI = 28
  KEEP_RUNNING = True  
  MSG_ACKNOWLEDGE = 128
  MSG_START_EXTERNAL = 129
  MULT_double2int = 1000000.0
  #---------------------------------
  # Search function options:
  DIN_SEARCH = 2
  global thread_flag_not_found = 0
  global joints_search = [0,0,0,0,0,0]
  thread Thread_movel_search():
    movel(joints_search, accel_mss, speed_ms, 0, blend_radius)
    global thread_flag_not_found = 1
  end
  
  
  #-----------------------------------------------------------------------------------
  # ROBOTIQ tools
  
  #aliases for the gripper variable names
  ACT = 1
  GTO = 2
  ATR = 3
  ARD = 4
  FOR = 5
  SPE = 6
  OBJ = 7
  STA = 8
  FLT = 9
  POS = 10
  PRE = 11
  
  def rq_init_connection(gripper_sid=9, gripper_socket="1"):
  	socket_open("127.0.0.1",63352, gripper_socket)
  	socket_set_var("SID", gripper_sid,  gripper_socket)
  	ack = socket_read_byte_list(3, gripper_socket)
  end
  
  def rq_set_sid(gripper_sid=9, gripper_socket="1"):
          socket_set_var("SID", gripper_sid,  gripper_socket)
          sync()
          return is_ack(socket_read_byte_list(3, gripper_socket))
  end
  
  def rq_activate(gripper_socket="1"):
  	rq_gripper_act = 0
  
          if (not rq_is_gripper_activated(gripper_socket)):
              rq_reset(gripper_socket)
          end
  
  	rq_set_var(ACT,1, gripper_socket)
  end
  
  def rq_activate_and_wait(gripper_socket="1"):
  	rq_activate(gripper_socket)
  
  	while(not rq_is_gripper_activated(gripper_socket)):
  		# wait for activation completed
  	end
  end
  
  def rq_stop(gripper_socket="1"):
  	rq_set_var(GTO,0, gripper_socket)
  end
  
  def rq_reset(gripper_socket="1"):
  	rq_gripper_act = 0
  	rq_obj_detect = 0
  	rq_mov_complete = 0
  
  	rq_set_var(ACT,0, gripper_socket)
  	rq_set_var(ATR,0, gripper_socket)
  end
  
  def rq_auto_release_open_and_wait(gripper_socket="1"):
  
  	rq_set_var(ARD,0, gripper_socket)
  	rq_set_var(ACT,1, gripper_socket)
  	rq_set_var(ATR,1, gripper_socket)
  
  	gFLT = rq_get_var(FLT, 2, gripper_socket)
  
  	while(not is_FLT_autorelease_completed(gFLT)):
  		gFLT = rq_get_var(FLT, 2, gripper_socket)
  	end
  end
  
  def rq_auto_release_close_and_wait(gripper_socket="1"):
  	rq_set_var(ARD,1, gripper_socket)
  	rq_set_var(ACT,1, gripper_socket)
  	rq_set_var(ATR,1, gripper_socket)
  
  	gFLT = rq_get_var(FLT, 2, gripper_socket)
  
  	while(not is_FLT_autorelease_completed(gFLT)):
  		gFLT = rq_get_var(FLT, 2, gripper_socket)
  	end
  end
  
  def rq_set_force(force, gripper_socket="1"):
  	rq_set_var(FOR, force, gripper_socket)
  end
  
  def rq_set_speed(speed, gripper_socket="1"):
  	rq_set_var(SPE, speed, gripper_socket)
  end
  
  def rq_open(gripper_socket="1"):
  	rq_move(0, gripper_socket)
  end
  
  def rq_close(gripper_socket="1"):
  	rq_move(255, gripper_socket)
  end
  
  def rq_open_and_wait(gripper_socket="1"):
  	rq_move_and_wait(0, gripper_socket)
  end
  
  def rq_close_and_wait(gripper_socket="1"):
  	rq_move_and_wait(255, gripper_socket)
  end
  
  def rq_move(pos, gripper_socket="1"):
  	rq_mov_complete = 0
  	rq_obj_detect = 0
  
  	rq_set_pos(pos, gripper_socket)
  	rq_go_to(gripper_socket)
  end
  
  def rq_move_and_wait(pos, gripper_socket="1"):
  	rq_move(pos, gripper_socket)
  
  	while (not rq_is_motion_complete(gripper_socket)):
  		# wait for motion completed
  		sleep(0.01)
  		sync()
  	end
  
  	# following code used for compatibility with previous versions
  	rq_is_object_detected(gripper_socket)
  
  	if (rq_obj_detect != 1):
  		rq_mov_complete = 1
  	end
  end
  
  def rq_wait(gripper_socket="1"):
          # Wait for the gripper motion to complete
          while (not rq_is_motion_complete(gripper_socket)):
  		# wait for motion completed
  		sleep(0.01)
  		sync()
  	end
  
  	# following code used for compatibility with previous versions
  	rq_is_object_detected(gripper_socket)
  
  	if (rq_obj_detect != 1):
  		rq_mov_complete = 1
  	end
  end
  
  def rq_go_to(gripper_socket="1"):
  	rq_set_var(GTO,1, gripper_socket)
  end
  
  # reset the rGTO to prevent movement and
  # set the position
  def rq_set_pos(pos, gripper_socket="1"):
  	rq_set_var(GTO,0, gripper_socket)
  
  	rq_set_var(POS, pos, gripper_socket)
  
  	gPRE = rq_get_var(PRE, 3, gripper_socket)
  	pre = (gPRE[1] - 48)*100 + (gPRE[2] -48)*10 + gPRE[3] - 48
  	sync()
  	while (pre != pos):
          rq_set_var(POS, pos, gripper_socket)
  		gPRE = rq_get_var(PRE, 3, gripper_socket)
  		pre = (gPRE[1] - 48)*100 + (gPRE[2] -48)*10 + gPRE[3] - 48
  		sync()
  	end
  end
  
  def rq_is_motion_complete(gripper_socket="1"):
  	rq_mov_complete = 0
  
  	gOBJ = rq_get_var(OBJ, 1, gripper_socket)
  	sleep(0.01)
  
  	if (is_OBJ_gripper_at_position(gOBJ)):
  		rq_mov_complete = 1
  		return True
  	end
  
  	if (is_OBJ_object_detected(gOBJ)):
  		rq_mov_complete = 1
  		return True
  	end
  
  	return False
  
  end
  
  def rq_is_gripper_activated(gripper_socket="1"):
  	gSTA = rq_get_var(STA, 1, gripper_socket)
  
  	if(is_STA_gripper_activated(gSTA)):
  		rq_gripper_act = 1
  		return True
  	else:
  		rq_gripper_act = 0
  		return False
  	end
  end
  
  def rq_is_object_detected(gripper_socket="1"):
  	gOBJ = rq_get_var(OBJ, 1, gripper_socket)
  
  	if(is_OBJ_object_detected(gOBJ)):
  		rq_obj_detect = 1
  		return True
  	else:
  		rq_obj_detect = 0
  		return False
  	end
  end
  
  def rq_current_pos(gripper_socket="1"):
  	rq_pos = socket_get_var("POS",gripper_socket)
  	sync()
      return rq_pos
  end
  
  def rq_print_gripper_fault_code(gripper_socket="1"):
  	gFLT = rq_get_var(FLT, 2, gripper_socket)
  
  	if(is_FLT_no_fault(gFLT)):
  		textmsg("Gripper Fault : ", "No Fault (0x00)")
  	elif (is_FLT_action_delayed(gFLT)):
  		textmsg("Gripper Fault : ", "Priority Fault: Action delayed, initialization must be completed prior to action (0x05)")
  	elif (is_FLT_not_activated(gFLT)):
  		textmsg("Gripper Fault : ", "Priority Fault: The activation must be set prior to action (0x07)")
  	elif (is_FLT_autorelease_in_progress(gFLT)):
  		textmsg("Gripper Fault : ", "Minor Fault: Automatic release in progress (0x0B)")
  	elif (is_FLT_overcurrent(gFLT)):
  		textmsg("Gripper Fault : ", "Minor Fault: Overcurrent protection tiggered (0x0E)")
  	elif (is_FLT_autorelease_completed(gFLT)):
  		textmsg("Gripper Fault : ", "Major Fault: Automatic release completed (0x0F)")
  	else:
  		textmsg("Gripper Fault : ", "Unkwown Fault")
  	end
  end
  
  def rq_print_gripper_num_cycles(gripper_socket="1"):
  	socket_send_string("GET NCY",gripper_socket)
  	sync()
  	string_from_server = socket_read_string(gripper_socket)
  	sync()
  
  	if(string_from_server == "0"):
  		textmsg("Gripper Cycle Number : ", "Number of cycles is unreachable.")
  	else:
  		textmsg("Gripper Cycle Number : ", string_from_server)
  	end
  end
  
  def rq_print_gripper_driver_state(gripper_socket="1"):
  	socket_send_string("GET DST",gripper_socket)
  	sync()
  	string_from_server = socket_read_string(gripper_socket)
  	sync()
  
  	if(string_from_server == "0"):
  		textmsg("Gripper Driver State : ", "RQ_STATE_INIT")
  	elif(string_from_server == "1"):
  		textmsg("Gripper Driver State : ", "RQ_STATE_LISTEN")
  	elif(string_from_server == "2"):
  		textmsg("Gripper Driver State : ", "RQ_STATE_READ_INFO")
  	elif(string_from_server == "3"):
  		textmsg("Gripper Driver State : ", "RQ_STATE_ACTIVATION")
  	else:
  		textmsg("Gripper Driver State : ", "RQ_STATE_RUN")
  	end
  end
  
  def rq_print_gripper_serial_number():
  	#socket_send_string("GET SNU",gripper_socket)
  	#sync()
  	#string_from_server = socket_read_string(gripper_socket)
  	#sync()
  	#textmsg("Gripper Serial Number : ", string_from_server)
  end
  
  def rq_print_gripper_firmware_version(gripper_socket="1"):
  	socket_send_string("GET FWV",gripper_socket)
  	sync()
  	string_from_server = socket_read_string(gripper_socket)
  	sync()
  	textmsg("Gripper Firmware Version : ", string_from_server)
  end
  
  def rq_print_gripper_driver_version(gripper_socket="1"):
  	socket_send_string("GET VER",gripper_socket)
  	sync()
  	string_from_server = socket_read_string(gripper_socket)
  	sync()
  	textmsg("Gripper Driver Version : ", string_from_server)
  end
  
  def rq_print_gripper_probleme_connection(gripper_socket="1"):
  	socket_send_string("GET PCO",gripper_socket)
  	sync()
  	string_from_server = socket_read_string(gripper_socket)
  	sync()
  	if (string_from_server == "0"):
  		textmsg("Gripper Connection State : ", "No connection problem detected")
  	else:
  		textmsg("Gripper Connection State : ", "Connection problem detected")
  	end
  end
  
  # Returns True if list_of_bytes is [3, 'a', 'c', 'k']
  def is_ack(list_of_bytes):
  
  	# list length is not 3
  	if (list_of_bytes[0] != 3):
  		return False
  	end
  
  	# first byte not is 'a'?
  	if (list_of_bytes[1] != 97):
  		return False
  	end
  
  	# first byte not is 'c'?
  	if (list_of_bytes[2] != 99):
  		return False
  	end
  
  	# first byte not is 'k'?
  	if (list_of_bytes[3] != 107):
  		return False
  	end
  
  	return True
  end
  
  # Returns True if list_of_bytes is not [3, 'a', 'c', 'k']
  def is_not_ack(list_of_bytes):
  	if (is_ack(list_of_bytes)):
  		return False
  	else:
  		return True
  	end
  end
  
  def is_STA_gripper_activated (list_of_bytes):
  
  	# list length is not 1
  	if (list_of_bytes[0] != 1):
  		return False
  	end
  
  	# byte is '3'?
  	if (list_of_bytes[1] == 51):
  		return True
  	end
  
  	return False
  end
  
  # Returns True if list_of_byte is [1, '1'] or [1, '2']
  # Used to test OBJ = 0x1 or OBJ = 0x2
  def is_OBJ_object_detected (list_of_bytes):
  
  	# list length is not 1
  	if (list_of_bytes[0] != 1):
  		return False
  	end
  
  	# byte is '2'?
  	if (list_of_bytes[1] == 50):
  		return True
  	end
  
  	# byte is '1'?
  	if (list_of_bytes[1]  == 49):
  		return True
  	end
  
  	return False
  
  end
  
  # Returns True if list_of_byte is [1, '3']
  # Used to test OBJ = 0x3
  def is_OBJ_gripper_at_position (list_of_bytes):
  
  	# list length is not 1
  	if (list_of_bytes[0] != 1):
  		return False
  	end
  
  	# byte is '3'?
  	if (list_of_bytes[1] == 51):
  		return True
  	end
  
  	return False
  end
  
  def is_not_OBJ_gripper_at_position (list_of_bytes):
  
  	if (is_OBJ_gripper_at_position(list_of_bytes)):
  		return False
  	else:
  		return True
  	end
  end
  
  def is_FLT_no_fault(list_of_bytes):
  
  	# list length is not 2
  	if (list_of_bytes[0] != 2):
  		return False
  	end
  
  	# first byte is '0'?
  	if (list_of_bytes[1] != 48):
  		return False
  	end
  
  	# second byte is '0'?
  	if (list_of_bytes[2] != 48):
  		return False
  	end
  
  	return True
  
  end
  
  def is_FLT_action_delayed(list_of_bytes):
  
  	# list length is not 2
  	if (list_of_bytes[0] != 2):
  		return False
  	end
  
  	# first byte is '0'?
  	if (list_of_bytes[1] != 48):
  		return False
  	end
  
  	# second byte is '5'?
  	if (list_of_bytes[2] != 53):
  		return False
  	end
  
  	return True
  end
  
  def is_FLT_not_activated(list_of_bytes):
  
  	# list length is not 2
  	if (list_of_bytes[0] != 2):
  		return False
  	end
  
  	# first byte is '0'?
  	if (list_of_bytes[1] != 48):
  		return False
  	end
  
  	# second byte is '7'?
  	if (list_of_bytes[2] != 55):
  		return False
  	end
  
  	return True
  end
  
  def is_FLT_autorelease_in_progress(list_of_bytes):
  
  	# list length is not 2
  	if (list_of_bytes[0] != 2):
  		return False
  	end
  
  	# first byte is '1'?
  	if (list_of_bytes[1] != 49):
  		return False
  	end
  
  	# second byte is '1'?
  	if (list_of_bytes[2] != 49):
  		return False
  	end
  
  	return True
  
  end
  
  def is_FLT_overcurrent(list_of_bytes):
  
  	# list length is not 2
  	if (list_of_bytes[0] != 2):
  		return False
  	end
  
  	# first byte is '1'?
  	if (list_of_bytes[1] != 49):
  		return False
  	end
  
  	# second byte is '4'?
  	if (list_of_bytes[2] != 52):
  		return False
  	end
  
  	return True
  
  end
  
  def is_FLT_autorelease_completed(list_of_bytes):
  
  	# list length is not 2
  	if (list_of_bytes[0] != 2):
  		return False
  	end
  
  	# first byte is '1'?
  	if (list_of_bytes[1] != 49):
  		return False
  	end
  
  	# second byte is '5'?
  	if (list_of_bytes[2] != 53):
  		return False
  	end
  
  	return True
  
  end
  
  def rq_set_var(var_name, var_value, gripper_socket="1"):
  
  	sync()
  	if (var_name == ACT):
  		socket_set_var("ACT", var_value, gripper_socket)
  	elif (var_name == GTO):
  		socket_set_var("GTO", var_value, gripper_socket)
  	elif (var_name == ATR):
  		socket_set_var("ATR", var_value, gripper_socket)
  	elif (var_name == ARD):
  		socket_set_var("ARD", var_value, gripper_socket)
  	elif (var_name == FOR):
  		socket_set_var("FOR", var_value, gripper_socket)
  	elif (var_name == SPE):
  		socket_set_var("SPE", var_value, gripper_socket)
  	elif (var_name == POS):
  		socket_set_var("POS", var_value, gripper_socket)
  	else:
  	end
  
  	sync()
  	ack = socket_read_byte_list(3, gripper_socket)
  	sync()
  
  	while(is_not_ack(ack)):
  
  		textmsg("rq_set_var : retry", " ...")
  		textmsg("rq_set_var : var_name = ", var_name)
  		textmsg("rq_set_var : var_value = ", var_value)
  
  		if (ack[0] != 0):
  			textmsg("rq_set_var : invalid ack value = ", ack)
  		end
  
  		socket_set_var(var_name , var_value,gripper_socket)
  		sync()
  		ack = socket_read_byte_list(3, gripper_socket)
  		sync()
  	end
  end
  
  
  def rq_get_var(var_name, nbr_bytes, gripper_socket="1"):
  
  	if (var_name == FLT):
  		socket_send_string("GET FLT",gripper_socket)
  		sync()
  	elif (var_name == OBJ):
  		socket_send_string("GET OBJ",gripper_socket)
  		sync()
  	elif (var_name == STA):
  		socket_send_string("GET STA",gripper_socket)
  		sync()
  	elif (var_name == PRE):
  		socket_send_string("GET PRE",gripper_socket)
  		sync()
  	else:
  	end
  
  	var_value = socket_read_byte_list(nbr_bytes, gripper_socket)
  	sync()
  
  	return var_value
  end
  
  ############################################
  # normalized functions (maps 0-100 to 0-255)
  ############################################
  def rq_set_force_norm(force_norm, gripper_socket="1"):
      force_gripper = norm_to_gripper(force_norm)
      rq_set_force(force_gripper, gripper_socket)
  end
  
  def rq_set_speed_norm(speed_norm, gripper_socket="1"):
      speed_gripper = norm_to_gripper(speed_norm)
      rq_set_speed(speed_gripper, gripper_socket)
  end
  
  def rq_move_norm(pos_norm, gripper_socket="1"):
      pos_gripper = norm_to_gripper(pos_norm)
      rq_move(pos_gripper, gripper_socket)
  end
  
  def rq_move_and_wait_norm(pos_norm, gripper_socket="1"):
      pos_gripper = norm_to_gripper(pos_norm)
      rq_move_and_wait(pos_gripper, gripper_socket)
  end
  
  def rq_set_pos_norm(pos_norm, gripper_socket="1"):
      pos_gripper = norm_to_gripper(pos_norm)
      rq_set_pos(pos_gripper, gripper_socket)
  end
  
  
  def rq_current_pos_norm(gripper_socket="1"):
      pos_gripper = rq_current_pos(gripper_socket)
      pos_norm = gripper_to_norm(pos_gripper)
      return pos_norm
  end
  
  def gripper_to_norm(value_gripper):
      value_norm = (value_gripper / 255) * 100
      return floor(value_norm)
  end
  
  def norm_to_gripper(value_norm):
      value_gripper = (value_norm / 100) * 255
      return ceil(value_gripper)
  end
  
  def rq_get_position():
      return rq_current_pos_norm()
  end
  #########################################
  rq_obj_detect = 0
  rq_init_connection(9, "1")
  connectivity_checked = [-1,-1,-1,-1]
  status_checked = [-1,-1,-1,-1]
  current_speed = [-1,-1,-1,-1]
  current_force = [-1,-1,-1,-1]
  
  #######Gripper URCap preamble end##########
  ###########################################
  
  # end: URCap Installation Node
  # begin: URCap Installation Node
  #   Source: Robotiq_Wrist_Camera, 1.2.1.R01, Robotiq Inc.
  #   Type: Camera
  
  ###########################################
  #######Vision urcap preamble start########
  
  logging_service = rpc_factory("xmlrpc","http://127.0.0.1:4747")
  # Converts a pose relative to the flange in the base frame.
  def get_T_in_base_from_flange(T_x_in_flange):
  
    T_flange_in_base = get_actual_tool_flange_pose()
  
    T_x_in_base = pose_trans(T_flange_in_base, T_x_in_flange)
  
    return T_x_in_base
  end
  
  # Search pose cartesian (camera pose)
  T_camera_in_flange = p[0.0, 0.05, 0.05, -0.5, 0.0, 0.0]
  snapshot_position_offset = p[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
  ignore_snapshot_position = False
  
  # Open connection with vision service
  xmlrpc_server=rpc_factory("xmlrpc","http://127.0.0.1:4242")
  
  #######Vision urcap preamble end##########
  ###########################################
  
  # end: URCap Installation Node
  # begin: URCap Installation Node
  #   Source: Robotiq_Copilot, 1.17.2.9189, Robotiq Inc.
  #   Type: Copilot
  # end: URCap Installation Node
  global ref_S3=p[0.0,0.0,0.0,0.0025,0.0081,1.5264]
  def init_prehenseur():
    rq_activate()
    sleep(4.0)
    if (connectivity_checked[0] != 1):
      if not(rq_set_sid(9, "1")):
        popup("Gripper 1 must be connected to run this program.", False, False, True)
      end
      connectivity_checked[0] = 1
    end
    if (status_checked[0] != 1):
      if not(rq_is_gripper_activated("1")):
        popup("Gripper 1 is not activated. Go to Installaton tab > Gripper to activate it and run the program again.", False, False, True)
      end
      status_checked[0] = 1
    end
    if (current_speed[0] != 2):
      rq_set_speed_norm(2, "1")
      current_speed[0] = 2
    end
    if (current_force[0] != 20):
      rq_set_force_norm(20, "1")
      current_force[0] = 20
    end
    rq_set_pos_norm(72, "1")
    rq_go_to("1")
    rq_wait("1")
    # end: URCap Program Node
  end
  
  # Start RQ Gripper
  init_prehenseur()
  # --------------------------------------------
  # --------------------------------------------
  # --------------------------------------------


  while KEEP_RUNNING:
    while socket_open(SERVER_IP, SERVER_PORT) == False:
      sleep(1)
    end
    while KEEP_RUNNING:
      cmd = socket_read_binary_integer(1)
      if cmd[0] == 0:
        # waiting
      elif cmd[0] > 1:
        popup("Received too many things","Error",False,True,blocking=True)
        break
      else:
        mtype = cmd[1]
        if mtype == MSG_SPEED:
          params_speed = socket_read_binary_integer(1)
          if params_speed[0] != 1:
            popup("One value expected","Error",False,True,blocking=True)
            break
          end
          speed_ms = params_speed[1] / MULT_double2int
          speed_rads = speed_ms
          socket_send_int(MSG_ACKNOWLEDGE)
        elif mtype == MSG_SPEED4:
          params_speed4 = socket_read_binary_integer(4)
          if params_speed4[0] != 4:
            popup("Four values expected","Error",False,True,blocking=True)
            break
          end
          if params_speed4[1] > 0:
            speed_ms = params_speed4[1] / MULT_double2int
          end
          if params_speed4[2] > 0:
            speed_rads = params_speed4[2] / MULT_double2int
          end
          if params_speed4[3] > 0:
            accel_mss = params_speed4[3] / MULT_double2int
          end
          if params_speed4[4] > 0:
            accel_radss = params_speed4[4] / MULT_double2int
          end
          socket_send_int(MSG_ACKNOWLEDGE)
        elif mtype == MSG_MOVEJ:
          params_movej = socket_read_binary_integer(6)
          if params_movej[0] != 6:
            popup("Six values expected (joints)","Error",False,True,blocking=True)
            break
          end
          q = [params_movej[1] / MULT_double2int, params_movej[2] / MULT_double2int, params_movej[3] / MULT_double2int, params_movej[4] / MULT_double2int, params_movej[5] / MULT_double2int, params_movej[6] / MULT_double2int]
          movej(q, accel_radss, speed_rads, 0, blend_radius)
          socket_send_int(MSG_ACKNOWLEDGE)
        elif mtype == MSG_MOVEJ_POSE:
          params_movej = socket_read_binary_integer(6)
          if params_movej[0] != 6:
            popup("Six values expected (pose)","Error",False,True,blocking=True)
            break
          end
          pose = p[params_movej[1] / MULT_double2int, params_movej[2] / MULT_double2int, params_movej[3] / MULT_double2int, params_movej[4] / MULT_double2int, params_movej[5] / MULT_double2int, params_movej[6] / MULT_double2int]
          movej(pose, accel_radss, speed_rads, 0, blend_radius)
          socket_send_int(MSG_ACKNOWLEDGE)
        elif mtype == MSG_MOVEL or mtype == MSG_MOVEP:
          params_movej = socket_read_binary_integer(6)
          if params_movej[0] != 6:
            popup("Six values expected (joints)","Error",False,True,blocking=True)
            break
          end
          q = [params_movej[1] / MULT_double2int, params_movej[2] / MULT_double2int, params_movej[3] / MULT_double2int, params_movej[4] / MULT_double2int, params_movej[5] / MULT_double2int, params_movej[6] / MULT_double2int]
          if mtype == MSG_MOVEL:
            movel(q, accel_mss, speed_ms, 0, blend_radius)
          else:
            movep(q, accel_mss, speed_ms, blend_radius)
          end
          socket_send_int(MSG_ACKNOWLEDGE)
        elif mtype == MSG_MOVEL_POSE or mtype == MSG_MOVEP_POSE:
          params_movej = socket_read_binary_integer(6)
          if params_movej[0] != 6:
            popup("Six values expected","Error",False,True,blocking=True)
            break
          end
          pose = p[params_movej[1] / MULT_double2int, params_movej[2] / MULT_double2int, params_movej[3] / MULT_double2int, params_movej[4] / MULT_double2int, params_movej[5] / MULT_double2int, params_movej[6] / MULT_double2int]
          if mtype == MSG_MOVEL_POSE:
            movel(pose, accel_mss, speed_ms, 0, blend_radius)
          else:
            movep(pose, accel_mss, speed_ms, blend_radius)
          end
          socket_send_int(MSG_ACKNOWLEDGE)
        elif mtype == MSG_MOVEL_SEARCH:
          params_movej = socket_read_binary_integer(6)
          if params_movej[0] != 6:
            popup("Six values expected","Error",False,True,blocking=True)
            break
          end
          joints_search = [params_movej[1] / MULT_double2int, params_movej[2] / MULT_double2int, params_movej[3] / MULT_double2int, params_movej[4] / MULT_double2int, params_movej[5] / MULT_double2int, params_movej[6] / MULT_double2int]
          thread_flag_not_found = 0
          thread_h_search = run Thread_movel_search()
          #while (get_standard_digital_in(DIN_SEARCH) == False and thread_flag_not_found == 0):
          while (get_standard_digital_out(DIN_SEARCH) == False and thread_flag_not_found == 0):
            sync()
          end
          if thread_flag_not_found == 0:
            q = get_actual_joint_positions()
            kill thread_h_search
          else:
            q = joints_search
          end
          sync()
          socket_send_int(q[0]*MULT_double2int)
          socket_send_int(q[1]*MULT_double2int)
          socket_send_int(q[2]*MULT_double2int)
          socket_send_int(q[3]*MULT_double2int)
          socket_send_int(q[4]*MULT_double2int)
          socket_send_int(q[5]*MULT_double2int)
          socket_send_int(MSG_ACKNOWLEDGE)
        elif mtype == MSG_MOVEC:
          params_movej = socket_read_binary_integer(6)
          if params_movej[0] != 6:
            popup("Two sets of joints expected (6+6 values)","Error",False,True,blocking=True)
            break
          end
          q1 = [params_movej[1] / MULT_double2int, params_movej[2] / MULT_double2int, params_movej[3] / MULT_double2int, params_movej[4] / MULT_double2int, params_movej[5] / MULT_double2int, params_movej[6] / MULT_double2int]
          params_movej = socket_read_binary_integer(6)
          if params_movej[0] != 6:
            popup("Two sets of joints expected (6+6 values)","Error",False,True,blocking=True)
            break
          end
          q2 = [params_movej[1] / MULT_double2int, params_movej[2] / MULT_double2int, params_movej[3] / MULT_double2int, params_movej[4] / MULT_double2int, params_movej[5] / MULT_double2int, params_movej[6] / MULT_double2int]
          movec(q1, q2, accel_mss, speed_ms, blend_radius)
          socket_send_int(MSG_ACKNOWLEDGE)
        elif mtype == MSG_MOVEC_POSE:
          params_movej = socket_read_binary_integer(6)
          if params_movej[0] != 6:
            popup("Two sets of poses expected (6+6 values)","Error",False,True,blocking=True)
            break
          end
          p1 = p[params_movej[1] / MULT_double2int, params_movej[2] / MULT_double2int, params_movej[3] / MULT_double2int, params_movej[4] / MULT_double2int, params_movej[5] / MULT_double2int, params_movej[6] / MULT_double2int]
          params_movej = socket_read_binary_integer(6)
          if params_movej[0] != 6:
            popup("Two sets of poses expected (6+6 values)","Error",False,True,blocking=True)
            break
          end
          p2 = p[params_movej[1] / MULT_double2int, params_movej[2] / MULT_double2int, params_movej[3] / MULT_double2int, params_movej[4] / MULT_double2int, params_movej[5] / MULT_double2int, params_movej[6] / MULT_double2int]
          movec(p1, p2, accel_mss, speed_ms, blend_radius)
          socket_send_int(MSG_ACKNOWLEDGE)
        elif mtype == MSG_CJNT:
          q = get_actual_joint_positions()
          socket_send_int(q[0]*MULT_double2int)
          socket_send_int(q[1]*MULT_double2int)
          socket_send_int(q[2]*MULT_double2int)
          socket_send_int(q[3]*MULT_double2int)
          socket_send_int(q[4]*MULT_double2int)
          socket_send_int(q[5]*MULT_double2int)
          socket_send_int(MSG_ACKNOWLEDGE)
        elif mtype == MSG_SETTOOL:
          tcp_xyzrpw = socket_read_binary_integer(6)
          if tcp_xyzrpw[0] != 6:
            popup("Six values expected","Error",False,True,blocking=True)
            break
          end
          new_tcp = p[tcp_xyzrpw[1] / MULT_double2int, tcp_xyzrpw[2] / MULT_double2int, tcp_xyzrpw[3] / MULT_double2int, tcp_xyzrpw[4] / MULT_double2int, tcp_xyzrpw[5] / MULT_double2int, tcp_xyzrpw[6] / MULT_double2int]
          set_tcp(new_tcp)
          socket_send_int(MSG_ACKNOWLEDGE)
        elif mtype == MSG_ROUNDING:
          values = socket_read_binary_integer(1)
          if values[0] != 1:
            popup("One values expected","Error",False,True,blocking=True)
            break
          end
          blend_radius = values[1] / MULT_double2int
          socket_send_int(MSG_ACKNOWLEDGE)
        elif mtype == MSG_POPUP:
          msg_popup = socket_read_string()
          popup(msg_popup, "Message", False,False,blocking=True)
          socket_send_int(MSG_ACKNOWLEDGE)
        elif mtype == MSG_PAUSE:
          params_pause = socket_read_binary_integer(1)
          if params_pause[0] != 1:
            popup("One value expected","Error",False,True,blocking=True)
            break
          end
          pause_ms = params_pause[1]
          if pause_ms <= 0:
            popup("Paused. Press continue to continue", "Message",False,False,blocking=True)
            sleep(2)
          else:
            sleep(pause_ms/1000.0)
          end
          socket_send_int(MSG_ACKNOWLEDGE)
        elif mtype == MSG_RUNPROG:
          # Run a program
          prog_num = socket_read_binary_integer(1)
          prog_name = socket_read_string()
          
          # we can run the following code and call the program by number in a customized way
          if prog_num[0] != 1:
            popup("One value expected","Error",False,True,blocking=True)
            break
          end
          prog_id = prog_num[1]
          
          
          if True:
            rq_move_and_wait(prog_id)
          end
          
          
          # List possible program names
          #if str_find(prog_name, "rq_move_and_wait") >= 0:
          #  rq_move_and_wait(prog_id)
          
          #elif str_find(prog_name, "rq_open") >= 0:
          #  rq_open()
            
          #elif str_find(prog_name, "rq_close") >= 0:
          #  rq_close()
            
          #end
          
          # Send message to signal completition
          socket_send_int(MSG_ACKNOWLEDGE)
          
        elif mtype == MSG_SETDO:
          params_out = socket_read_binary_integer(2)
          if params_out[0] != 2:
            popup("Two values expected","Error",False,True,blocking=True)
            break
          end
          io_name = params_out[1]
          io_value = params_out[2]          
          if io_value > 0:
            set_standard_digital_out(io_name, True)
          else:
            set_standard_digital_out(io_name, False)
          end
          socket_send_int(MSG_ACKNOWLEDGE)
        elif mtype == MSG_SETAO:
          params_out = socket_read_binary_integer(2)
          if params_out[0] != 2:
            popup("Two values expected","Error",False,True,blocking=True)
            break
          end
          io_name = params_out[1]
          io_value = params_out[2] / MULT_double2int       
          set_standard_analog_out(io_name, io_value)
          socket_send_int(MSG_ACKNOWLEDGE)
        elif mtype == MSG_GETDI:
          params_in = socket_read_binary_integer(1)
          if params_in[0] != 1:
            popup("One value expected","Error",False,True,blocking=True)
            break
          end
          io_name = params_in[1]        
          if get_standard_digital_in(io_name):
            socket_send_int(MULT_double2int)          
          else:
            socket_send_int(0)
          end
          socket_send_int(MSG_ACKNOWLEDGE)
        elif mtype == MSG_GETAI:
          params_in = socket_read_binary_integer(1)
          if params_in[0] != 1:
            popup("One value expected","Error",False,True,blocking=True)
            break
          end
          io_name = params_in[1]        
          socket_send_int(get_standard_analog_in(io_name)*MULT_double2int)          
          socket_send_int(MSG_ACKNOWLEDGE)
        elif mtype == MSG_WAITDI:
          params_out = socket_read_binary_integer(2)
          if params_out[0] != 2:
            popup("Two values expected","Error",False,True,blocking=True)
            break
          end
          io_name = params_out[1]
          io_value = params_out[2]
          if io_value > 0:
            while (get_standard_digital_in(io_name) == False):
              sync()
            end
          else:
            while (get_standard_digital_in(io_name) == True):
              sync()
            end
          end
          socket_send_int(MSG_ACKNOWLEDGE)
        else:
          popup("Unexpected instruction restarting communications.","Communication problems",False,True,blocking=True)
          break
        end
      end
    end
  end
end
DriverRoboDK()