NEW_LINE = "\n" QUOTATION = "\"" ROBOTIQ_PREAMBLE = "\n".join([ "#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", "LBP = 12", "LRD = 13", "LBL = 14", "LGN = 15", "MSC = 16", "MOD = 17", "", "gripper_1_connected = False", "gripper_2_connected = False", "gripper_3_connected = False", "gripper_4_connected = False", "", "gripper_1_socket_open = False", "gripper_2_socket_open = False", "gripper_3_socket_open = False", "gripper_4_socket_open = False", "", "def rq_init_comm_if_connected(gripper_sid=9, gripper_socket=" + QUOTATION + "1" + QUOTATION + "):", " if(not is_gripper_socket_open(gripper_socket)):", " open_gripper_socket(gripper_socket)", " end", "", " is_gripper_connected = rq_is_gripper_connected(gripper_sid, gripper_socket)", " if(is_gripper_connected):", " rq_set_gripper_connected(gripper_socket)", " end", "", " return is_gripper_connected", "end", "", "def open_gripper_socket(gripper_socket):", " is_open = socket_open(" + QUOTATION + "127.0.0.1" + QUOTATION + ",63352, gripper_socket)", " set_gripper_socket_open(gripper_socket, is_open)", "end", "", "def is_gripper_socket_open(gripper_socket):", " if(gripper_socket == " + QUOTATION + "1" + QUOTATION + "):", " return gripper_1_socket_open", " elif(gripper_socket == " + QUOTATION + "2" + QUOTATION + "):", " return gripper_2_socket_open", " elif(gripper_socket == " + QUOTATION + "3" + QUOTATION + "):", " return gripper_3_socket_open", " elif(gripper_socket == " + QUOTATION + "4" + QUOTATION + "):", " return gripper_4_socket_open", " else:", " return False", " end", "end", "", "def set_gripper_socket_open(gripper_socket, is_open):", " if(gripper_socket == " + QUOTATION + "1" + QUOTATION + "):", " gripper_1_socket_open = is_open", " elif(gripper_socket == " + QUOTATION + "2" + QUOTATION + "):", " gripper_2_socket_open = is_open", " elif(gripper_socket == " + QUOTATION + "3" + QUOTATION + "):", " gripper_3_socket_open = is_open", " elif(gripper_socket == " + QUOTATION + "4" + QUOTATION + "):", " gripper_4_socket_open = is_open", " else:", " end", "end", "", "def rq_is_gripper_connected(gripper_sid=9, gripper_socket=" + QUOTATION + "1" + QUOTATION + "):", " socket_set_var(" + QUOTATION + "SID" + QUOTATION + ", gripper_sid, gripper_socket)", " ack = socket_read_byte_list(3, gripper_socket, 0.1)", " return is_ack(ack)", "end", "", "def rq_set_gripper_connected(gripper_id=" + QUOTATION + "1" + QUOTATION + "):", " if(gripper_id == " + QUOTATION + "1" + QUOTATION + "):", " gripper_1_connected = True", " end", "", " if (gripper_id == " + QUOTATION + "2" + QUOTATION + "):", " gripper_2_connected = True", " end", "", " if (gripper_id == " + QUOTATION + "3" + QUOTATION + "):", " gripper_3_connected = True", " end", "", " if (gripper_id == " + QUOTATION + "4" + QUOTATION + "):", " gripper_4_connected = True", " end", "end", "", "def rq_wait_for_gripper_connected():", " gripper_socket = " + QUOTATION + "gripper_conn_socket" + QUOTATION + "", " socket_open(" + QUOTATION + "127.0.0.1" + QUOTATION + ",63352, gripper_socket)", "", " retryCtr = 1", " sid_list = rq_get_sid(gripper_socket)", " gripper_is_connected = rq_is_any_gripper_connected(sid_list)", "", " while(not gripper_is_connected and retryCtr < 2000):", " retryCtr = retryCtr + 1", " sid_list = rq_get_sid(gripper_socket)", " gripper_is_connected = rq_is_any_gripper_connected(sid_list)", " end", "", " socket_close(gripper_socket)", "end", "", "def rq_is_any_gripper_connected(sid_list):", " is_gripper_1_connected = rq_is_gripper1_in_sid_list(sid_list)", " is_gripper_2_connected = rq_is_gripper2_in_sid_list(sid_list)", " is_gripper_3_connected = rq_is_gripper3_in_sid_list(sid_list)", " is_gripper_4_connected = rq_is_gripper4_in_sid_list(sid_list)", "", " if(is_gripper_1_connected or is_gripper_2_connected or is_gripper_3_connected or is_gripper_4_connected):", " return True", " else:", " return False", " end", "end", "", "def rq_is_gripper_in_sid_list(gripper_sid, sid_list):", " sid_list_length = sid_list[0]", " sid_list_empty_length = 2", "", " if (sid_list_length <= sid_list_empty_length):", " return False", " end", "", " sid1 = sid_list[2]", " sid2 = sid_list[5]", " sid3 = sid_list[8]", " sid4 = sid_list[11]", "", " if(sid1 == gripper_sid or sid2 == gripper_sid or sid3 == gripper_sid or sid4 == gripper_sid):", " return True", " else:", " return False", " end", "end", "", "def rq_is_gripper1_in_sid_list(sid_list):", " gripper_1_sid_ascii = 57", " return rq_is_gripper_in_sid_list(gripper_1_sid_ascii, sid_list)", "end", "", "def rq_is_gripper2_in_sid_list(sid_list):", " gripper_2_sid_ascii = 50", " return rq_is_gripper_in_sid_list(gripper_2_sid_ascii, sid_list)", "end", "", "def rq_is_gripper3_in_sid_list(sid_list):", " gripper_3_sid_ascii = 51", " return rq_is_gripper_in_sid_list(gripper_3_sid_ascii, sid_list)", "end", "", "def rq_is_gripper4_in_sid_list(sid_list):", " gripper_4_sid_ascii = 52", " return rq_is_gripper_in_sid_list(gripper_4_sid_ascii, sid_list)", "end", "", "def rq_set_sid(gripper_sid=9, gripper_socket=" + QUOTATION + "1" + QUOTATION + "):", " socket_set_var(" + QUOTATION + "SID" + QUOTATION + ", gripper_sid, gripper_socket)", " sync()", " ack = socket_read_byte_list(3, gripper_socket)", " return is_ack(ack)", "end", "", "def rq_get_sid(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):", " socket_send_string(" + QUOTATION + "GET SID" + QUOTATION + ", gripper_socket)", " sync()", " sid_list = socket_read_byte_list(17, gripper_socket)", " sync()", " return sid_list", "end", "", "def rq_activate(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):", " rq_gripper_act = 0", "", " if (not rq_is_gripper_activated(gripper_socket)):", " rq_reset(gripper_socket)", "", " while(socket_get_var(" + QUOTATION + "ACT" + QUOTATION + ",gripper_socket) == 1):", " sleep(0.1)", " rq_reset(gripper_socket)", " end", " end", "", " rq_set_var(ACT,1, gripper_socket)", "end", "", "def rq_activate_and_wait(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):", " rq_activate(gripper_socket)", " sleep(1.0)", "", " while(not rq_is_gripper_activated(gripper_socket)):", " # wait for activation completed", " end", " sleep(0.5)", "end", "", "def rq_activate_all_grippers(reset=False):", " if(gripper_1_connected):", " rq_reset_and_activate(" + QUOTATION + "1" + QUOTATION + ", reset)", " end", "", " if(gripper_2_connected):", " rq_reset_and_activate(" + QUOTATION + "2" + QUOTATION + ", reset)", " end", "", " if(gripper_3_connected):", " rq_reset_and_activate(" + QUOTATION + "3" + QUOTATION + ", reset)", " end", "", " if(gripper_4_connected):", " rq_reset_and_activate(" + QUOTATION + "4" + QUOTATION + ", reset)", " end", "", " sleep(0.2)", "end", "", "def rq_reset_and_activate(gripper_socket=" + QUOTATION + "1" + QUOTATION + ", reset=False):", " if(reset):", " rq_reset(gripper_socket)", " sleep(0.5)", " rq_activate_and_wait(gripper_socket)", " elif(not rq_is_gripper_activated(gripper_socket)):", " rq_activate_and_wait(gripper_socket)", " end", "end", "", "def rq_scan_block():", " gripper_socket = " + QUOTATION + "scn_block_socket" + QUOTATION + "", " socket_open(" + QUOTATION + "127.0.0.1" + QUOTATION + ", 63352, gripper_socket)", " socket_set_var(" + QUOTATION + "SCN_BLOCK" + QUOTATION + ", 1, gripper_socket)", " sync()", " ack_test = socket_read_byte_list(3, gripper_socket)", "", " retry_counter = 0", "", " while(not is_ack(ack_test) and retry_counter < 5):", " socket_set_var(" + QUOTATION + "SCN_BLOCK" + QUOTATION + ", 1, gripper_socket)", " sync()", " ack_test = socket_read_byte_list(3, gripper_socket)", " retry_counter = retry_counter + 1", " end", "", " socket_close(" + QUOTATION + "scn_block_socket" + QUOTATION + ")", "end", "", "def rq_reset(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):", " 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=" + QUOTATION + "1" + QUOTATION + "):", " rq_set_var(ARD,0, gripper_socket)", " rq_set_var(ACT,1, gripper_socket)", " rq_set_var(ATR,0, gripper_socket)", " sleep(0.1)", " rq_set_var(ATR,1, gripper_socket)", "", " rq_wait_autorelease_completed(gripper_socket)", "end", "", "def rq_auto_release_close_and_wait(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):", " rq_set_var(ARD,1, gripper_socket)", " rq_set_var(ACT,1, gripper_socket)", " rq_set_var(ATR,0, gripper_socket)", " sleep(0.1)", " rq_set_var(ATR,1, gripper_socket)", "", " rq_wait_autorelease_completed(gripper_socket)", "end", "", "def rq_wait_autorelease_completed(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):", " retryCounter = 1", " gFLT = rq_get_var(FLT, 2, gripper_socket)", "", " while(not is_FLT_autorelease_in_progress(gFLT) and retryCounter <= 20):", " retryCounter = retryCounter + 1", " gFLT = rq_get_var(FLT, 2, gripper_socket)", " sleep(0.1)", " end", "", " retryCounter = 1", " gFLT = rq_get_var(FLT, 2, gripper_socket)", "", " while(not is_FLT_autorelease_completed(gFLT) and retryCounter <= 100):", " retryCounter = retryCounter + 1", " gFLT = rq_get_var(FLT, 2, gripper_socket)", " sleep(0.1)", " end", "end", "", "def rq_set_force(force, gripper_socket=" + QUOTATION + "1" + QUOTATION + "):", " force = floor(scale(force, [0, 255], [0,255]))", " rq_set_var(FOR, force, gripper_socket)", "end", "", "def rq_set_speed(speed, gripper_socket=" + QUOTATION + "1" + QUOTATION + "):", " speed = floor(scale(speed, [0, 255], [0,255]))", " rq_set_var(SPE, speed, gripper_socket)", "end", "", "def rq_open(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):", " rq_move(0, gripper_socket)", "end", "", "def rq_close(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):", " rq_move(255, gripper_socket)", "end", "", "def rq_open_and_wait(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):", " rq_move_and_wait(0, gripper_socket)", "end", "", "def rq_close_and_wait(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):", " rq_move_and_wait(255, gripper_socket)", "end", "", "def rq_move(pos, gripper_socket=" + QUOTATION + "1" + QUOTATION + "):", " 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=" + QUOTATION + "1" + QUOTATION + "):", " 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_for_pos_request(pos, gripper_socket=" + QUOTATION + "1" + QUOTATION + "):", " gPRE = rq_get_var(PRE, 3, gripper_socket)", " pre = (gPRE[1] - 48)*100 + (gPRE[2] -48)*10 + gPRE[3] - 48", "", " 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_wait_for_pos(pos, gripper_socket=" + QUOTATION + "1" + QUOTATION + "):", " rq_wait_for_pos_request(pos, gripper_socket)", "", " # Wait for the gripper motion to complete", " while (not rq_is_motion_complete(gripper_socket)):", " # wait for motion completed", " sleep(0.01)", " sync()", " rq_go_to(gripper_socket)", " 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=" + QUOTATION + "1" + QUOTATION + "):", " # 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_wait_for_object_detected(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):", " # Wait the object detection", " while (not rq_is_object_detected(gripper_socket)):", " # wait for motion completed", " sleep(0.01)", " sync()", " end", "end", "", "# set the position", "def rq_set_pos(pos, gripper_socket=" + QUOTATION + "1" + QUOTATION + "):", " pos = floor(scale(pos, [0, 255], [0, 255]))", " rq_set_var(POS, pos, gripper_socket)", " rq_wait_for_pos_request(pos, gripper_socket)", "end", "", "# set the position, speed and force", "def rq_set_pos_spd_for(pos, speed, force, gripper_socket=" + QUOTATION + "1" + QUOTATION + "):", " enter_critical", " rq_send_pos_spd_for(pos, speed, force, gripper_socket)", " ack = socket_read_byte_list(3, gripper_socket)", " exit_critical", "", " sync()", "", " while(is_not_ack(ack)):", " enter_critical", " rq_send_pos_spd_for(pos, speed, force, gripper_socket)", " ack = socket_read_byte_list(3, gripper_socket)", " exit_critical", "", " sync()", " end", "", " rq_wait_for_pos_request(pos, gripper_socket)", "end", "", "def rq_set_gripper_max_current_mA(current_mA, gripper_socket=" + QUOTATION + "1" + QUOTATION + "):", " current = floor(current_mA / 10)", " rq_set_var(MSC, current, gripper_socket)", " sleep(1.5)", "end", "", "def rq_set_gripper_mode(mode, gripper_socket=" + QUOTATION + "1" + QUOTATION + "):", " rq_set_var(MOD, mode, gripper_socket)", "end", "", "def rq_set_gripper_max_cur(current_mA, gripper_socket=" + QUOTATION + "1" + QUOTATION + "):", " rq_set_gripper_max_current_mA(current_mA, gripper_socket)", "end", "", "def rq_get_gripper_max_current_mA(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):", " socket_send_string(" + QUOTATION + "GET MSC" + QUOTATION + ",gripper_socket)", " sync()", " var_value = socket_read_byte_list(3, gripper_socket)", "", " current = rq_list_of_bytes_to_value(var_value)", "", " if(current == -1):", " current_mA = current", " else:", " current_mA = current * 10", " end", "", " return current_mA", "end", "", "def rq_get_gripper_max_cur(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):", " return rq_get_gripper_max_current_mA(gripper_socket)", "end", "", "def rq_list_of_bytes_to_value(list_of_bytes):", " value = -1", "", " # response list length", " if (list_of_bytes[0] == 1):", " value = list_of_bytes[1] - 48", " elif (list_of_bytes[0] == 2):", " value = (list_of_bytes[1] - 48) * 10 + (list_of_bytes[2] - 48)", " elif (list_of_bytes[0] == 3):", " value = (list_of_bytes[1] - 48) * 100 + (list_of_bytes[2] - 48) * 10 + (list_of_bytes[3] - 48)", " end", "", " return value", "end", "", "# send the position, speed and force", "def rq_send_pos_spd_for(pos, speed, force, gripper_socket=" + QUOTATION + "1" + QUOTATION + "):", " pos = floor(scale(pos, [0, 255], [0, 255]))", " speed = floor(scale(speed, [0, 255], [0, 255]))", " force = floor(scale(force, [0, 255], [0, 255]))", "", " socket_send_string(" + QUOTATION + "SET POS" + QUOTATION + ", gripper_socket)", " socket_send_byte(32, gripper_socket)", " socket_send_string(pos, gripper_socket)", " socket_send_byte(32, gripper_socket)", " socket_send_string(" + QUOTATION + "SPE" + QUOTATION + ", gripper_socket)", " socket_send_byte(32, gripper_socket)", " socket_send_string(speed, gripper_socket)", " socket_send_byte(32, gripper_socket)", " socket_send_string(" + QUOTATION + "FOR" + QUOTATION + ", gripper_socket)", " socket_send_byte(32, gripper_socket)", " socket_send_string(force, gripper_socket)", " socket_send_byte(10, gripper_socket)", "end", "", "def rq_is_motion_complete(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):", " 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=" + QUOTATION + "1" + QUOTATION + "):", " 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=" + QUOTATION + "1" + QUOTATION + "):", " 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=" + QUOTATION + "1" + QUOTATION + "):", " enter_critical", " rq_pos = socket_get_var(" + QUOTATION + "POS" + QUOTATION + ",gripper_socket)", " exit_critical", " sync()", " return rq_pos", "end", "", "def rq_motor_current(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):", " enter_critical", " rq_current = socket_get_var(" + QUOTATION + "COU" + QUOTATION + ",gripper_socket)", " exit_critical", " sync()", " return rq_current * 10", "end", "", "def rq_print_connected_grippers():", " if(gripper_1_connected):", " textmsg(" + QUOTATION + "Gripper 1 : " + QUOTATION + ", " + QUOTATION + "connected and socket open." + QUOTATION + ")", " end", "", " if (gripper_2_connected):", " textmsg(" + QUOTATION + "Gripper 2 : " + QUOTATION + ", " + QUOTATION + "connected and socket open." + QUOTATION + ")", " end", "", " if (gripper_3_connected):", " textmsg(" + QUOTATION + "Gripper 3 : " + QUOTATION + ", " + QUOTATION + "connected and socket open." + QUOTATION + ")", " end", "", " if (gripper_4_connected):", " textmsg(" + QUOTATION + "Gripper 4 : " + QUOTATION + ", " + QUOTATION + "connected and socket open." + QUOTATION + ")", " end", "end", "", "def rq_print_gripper_fault_code(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):", " gFLT = rq_get_var(FLT, 2, gripper_socket)", "", " if(is_FLT_no_fault(gFLT)):", " textmsg(" + QUOTATION + "Gripper Fault : " + QUOTATION + ", " + QUOTATION + "No Fault (0x00)" + QUOTATION + ")", " elif (is_FLT_action_delayed(gFLT)):", " textmsg(" + QUOTATION + "Gripper Fault : " + QUOTATION + ", " + QUOTATION + "Priority Fault: Action delayed, initialization must be completed prior to action (0x05)" + QUOTATION + ")", " elif (is_FLT_not_activated(gFLT)):", " textmsg(" + QUOTATION + "Gripper Fault : " + QUOTATION + ", " + QUOTATION + "Priority Fault: The activation must be set prior to action (0x07)" + QUOTATION + ")", " elif (is_FLT_autorelease_in_progress(gFLT)):", " textmsg(" + QUOTATION + "Gripper Fault : " + QUOTATION + ", " + QUOTATION + "Minor Fault: Automatic release in progress (0x0B)" + QUOTATION + ")", " elif (is_FLT_overcurrent(gFLT)):", " textmsg(" + QUOTATION + "Gripper Fault : " + QUOTATION + ", " + QUOTATION + "Minor Fault: Overcurrent protection triggered (0x0E)" + QUOTATION + ")", " elif (is_FLT_autorelease_completed(gFLT)):", " textmsg(" + QUOTATION + "Gripper Fault : " + QUOTATION + ", " + QUOTATION + "Major Fault: Automatic release completed (0x0F)" + QUOTATION + ")", " else:", " textmsg(" + QUOTATION + "Gripper Fault : " + QUOTATION + ", " + QUOTATION + "Unknown Fault" + QUOTATION + ")", " end", "end", "", "def rq_print_gripper_num_cycles(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):", " socket_send_string(" + QUOTATION + "GET NCY" + QUOTATION + ",gripper_socket)", " sync()", " string_from_server = socket_read_string(gripper_socket)", " sync()", "", " if(string_from_server == " + QUOTATION + "0" + QUOTATION + "):", " textmsg(" + QUOTATION + "Gripper Cycle Number : " + QUOTATION + ", " + QUOTATION + "Number of cycles is unreachable." + QUOTATION + ")", " else:", " textmsg(" + QUOTATION + "Gripper Cycle Number : " + QUOTATION + ", string_from_server)", " end", "end", "", "def rq_print_gripper_driver_state(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):", " socket_send_string(" + QUOTATION + "GET DST" + QUOTATION + ",gripper_socket)", " sync()", " string_from_server = socket_read_string(gripper_socket)", " sync()", "", " if(string_from_server == " + QUOTATION + "0" + QUOTATION + "):", " textmsg(" + QUOTATION + "Gripper Driver State : " + QUOTATION + ", " + QUOTATION + "RQ_STATE_INIT" + QUOTATION + ")", " elif(string_from_server == " + QUOTATION + "1" + QUOTATION + "):", " textmsg(" + QUOTATION + "Gripper Driver State : " + QUOTATION + ", " + QUOTATION + "RQ_STATE_LISTEN" + QUOTATION + ")", " elif(string_from_server == " + QUOTATION + "2" + QUOTATION + "):", " textmsg(" + QUOTATION + "Gripper Driver State : " + QUOTATION + ", " + QUOTATION + "RQ_STATE_READ_INFO" + QUOTATION + ")", " elif(string_from_server == " + QUOTATION + "3" + QUOTATION + "):", " textmsg(" + QUOTATION + "Gripper Driver State : " + QUOTATION + ", " + QUOTATION + "RQ_STATE_ACTIVATION" + QUOTATION + ")", " else:", " textmsg(" + QUOTATION + "Gripper Driver State : " + QUOTATION + ", " + QUOTATION + "RQ_STATE_RUN" + QUOTATION + ")", " end", "end", "", "def rq_print_gripper_serial_number(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):", " socket_send_string(" + QUOTATION + "GET SNU" + QUOTATION + ",gripper_socket)", " sync()", " string_from_server = socket_read_string(gripper_socket)", " sync()", " textmsg(" + QUOTATION + "Gripper Serial Number : " + QUOTATION + ", string_from_server)", "end", "", "def rq_print_gripper_firmware_version(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):", " socket_send_string(" + QUOTATION + "GET FWV" + QUOTATION + ",gripper_socket)", " sync()", " string_from_server = socket_read_string(gripper_socket)", " sync()", " textmsg(" + QUOTATION + "Gripper Firmware Version : " + QUOTATION + ", string_from_server)", "end", "", "def rq_print_gripper_driver_version(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):", " socket_send_string(" + QUOTATION + "GET VER" + QUOTATION + ",gripper_socket)", " sync()", " string_from_server = socket_read_string(gripper_socket)", " sync()", " textmsg(" + QUOTATION + "Gripper Driver Version : " + QUOTATION + ", string_from_server)", "end", "", "def rq_print_gripper_probleme_connection(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):", " socket_send_string(" + QUOTATION + "GET PCO" + QUOTATION + ",gripper_socket)", " sync()", " string_from_server = socket_read_string(gripper_socket)", " sync()", " if (string_from_server == " + QUOTATION + "0" + QUOTATION + "):", " textmsg(" + QUOTATION + "Gripper Connection State : " + QUOTATION + ", " + QUOTATION + "No connection problem detected" + QUOTATION + ")", " else:", " textmsg(" + QUOTATION + "Gripper Connection State : " + QUOTATION + ", " + QUOTATION + "Connection problem detected" + QUOTATION + ")", " 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", "", "#### GTO Section ####", "def rq_stop(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):", " rq_set_var(GTO, 0, gripper_socket)", "end", "", "def rq_set_GTO_and_wait(value, gripper_socket=" + QUOTATION + "1" + QUOTATION + "):", " rq_set_var(GTO ,value, gripper_socket)", " while(not is_GTO(value, rq_get_var(GTO, 1, gripper_socket))):", " sync()", " end", "end", "", "def rq_go_to(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):", " rq_set_var(GTO, 1, gripper_socket)", "end", "", "", "def is_GTO(goto_value, list_of_bytes):", " zero_ascii = 48", " if (list_of_bytes[0] != 1):", " return False", " end", "", " if (list_of_bytes[1] == zero_ascii + goto_value):", " return True", " else:", " return False", " end", "end", "#### GTO Section ####", "", "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=" + QUOTATION + "1" + QUOTATION + "):", "", " var_name_string = " + QUOTATION + "" + QUOTATION + "", "", " if (var_name == ACT):", " var_name_string = " + QUOTATION + "ACT" + QUOTATION + "", " elif (var_name == GTO):", " var_name_string = " + QUOTATION + "GTO" + QUOTATION + "", " elif (var_name == ATR):", " var_name_string = " + QUOTATION + "ATR" + QUOTATION + "", " elif (var_name == ARD):", " var_name_string = " + QUOTATION + "ARD" + QUOTATION + "", " elif (var_name == FOR):", " var_name_string = " + QUOTATION + "FOR" + QUOTATION + "", " elif (var_name == SPE):", " var_name_string = " + QUOTATION + "SPE" + QUOTATION + "", " elif (var_name == POS):", " var_name_string = " + QUOTATION + "POS" + QUOTATION + "", " elif (var_name == LBP):", " var_name_string = " + QUOTATION + "LBP" + QUOTATION + "", " elif (var_name == LRD):", " var_name_string = " + QUOTATION + "LRD" + QUOTATION + "", " elif (var_name == LBL):", " var_name_string = " + QUOTATION + "LBL" + QUOTATION + "", " elif (var_name == LGN):", " var_name_string = " + QUOTATION + "LGN" + QUOTATION + "", " elif (var_name == MSC):", " var_name_string = " + QUOTATION + "MSC" + QUOTATION + "", " elif (var_name == MOD):", " var_name_string = " + QUOTATION + "MOD" + QUOTATION + "", " end", "", " enter_critical", " socket_set_var(var_name_string, var_value, gripper_socket)", " ack = socket_read_byte_list(3, gripper_socket)", " exit_critical", "", " sync()", "", " while(is_not_ack(ack)):", " enter_critical", " socket_set_var(var_name_string , var_value, gripper_socket)", " sync()", " ack = socket_read_byte_list(3, gripper_socket)", " exit_critical", "", " sync()", " end", "end", "", "", "def rq_get_var(var_name, nbr_bytes, gripper_socket=" + QUOTATION + "1" + QUOTATION + "):", " enter_critical", "", " if (var_name == FLT):", " socket_send_string(" + QUOTATION + "GET FLT" + QUOTATION + ", gripper_socket)", " elif (var_name == OBJ):", " socket_send_string(" + QUOTATION + "GET OBJ" + QUOTATION + ", gripper_socket)", " elif (var_name == STA):", " socket_send_string(" + QUOTATION + "GET STA" + QUOTATION + ", gripper_socket)", " elif (var_name == PRE):", " socket_send_string(" + QUOTATION + "GET PRE" + QUOTATION + ", gripper_socket)", " elif (var_name == GTO):", " socket_send_string(" + QUOTATION + "GET GTO" + QUOTATION + ", gripper_socket)", " else:", " end", "", " var_value = socket_read_byte_list(nbr_bytes, gripper_socket)", "", " exit_critical", "", " sync()", "", " return var_value", "end", "", "def rq_is_object_validated(gripper_selected, gripper_socket=" + QUOTATION + "1" + QUOTATION + "):", " if(gripper_selected):", " if(rq_is_object_detected(gripper_socket)):", " return True", " else:", " return False", " end", " else:", " return True", " end", "end", "", "############################################", "# normalized functions (maps 0-100 to 0-255)", "############################################", "def rq_set_force_norm(force_norm, gripper_socket=" + QUOTATION + "1" + QUOTATION + "):", " force_gripper = norm_to_gripper(force_norm)", " rq_set_force(force_gripper, gripper_socket)", "end", "", "def rq_set_speed_norm(speed_norm, gripper_socket=" + QUOTATION + "1" + QUOTATION + "):", " speed_gripper = norm_to_gripper(speed_norm)", " rq_set_speed(speed_gripper, gripper_socket)", "end", "", "def rq_move_norm(pos_norm, gripper_socket=" + QUOTATION + "1" + QUOTATION + "):", " pos_gripper = norm_to_gripper(pos_norm)", " rq_move(pos_gripper, gripper_socket)", "end", "", "def rq_move_and_wait_norm(pos_norm, gripper_socket=" + QUOTATION + "1" + QUOTATION + "):", " 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=" + QUOTATION + "1" + QUOTATION + "):", " pos_gripper = norm_to_gripper(pos_norm)", " rq_set_pos(pos_gripper, gripper_socket)", "end", "", "def rq_current_pos_norm(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):", " 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", "", "def rq_gripper_led_on(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):", " rq_set_var(LBP,0, gripper_socket)", "end", "", "def rq_gripper_led_off(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):", " rq_set_var(LBP,1, gripper_socket)", " rq_set_var(LRD,0, gripper_socket)", " rq_set_var(LBL,0, gripper_socket)", " rq_set_var(LGN,0, gripper_socket)", "end", "", "def rq_gripper_led_force_red(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):", " rq_set_var(LBP,1, gripper_socket)", " rq_set_var(LRD,1, gripper_socket)", " rq_set_var(LBL,0, gripper_socket)", " rq_set_var(LGN,0, gripper_socket)", "end", "", "def rq_gripper_led_force_blue(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):", " rq_set_var(LBP,1, gripper_socket)", " rq_set_var(LRD,0, gripper_socket)", " rq_set_var(LBL,1, gripper_socket)", " rq_set_var(LGN,0, gripper_socket)", "end", "", "def rq_gripper_led_force_green(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):", " rq_set_var(LBP,1, gripper_socket)", " rq_set_var(LRD,0, gripper_socket)", " rq_set_var(LBL,0, gripper_socket)", " rq_set_var(LGN,1, gripper_socket)", "end", "", "def rq_gripper_led_force_purple(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):", " rq_set_var(LBP,1, gripper_socket)", " rq_set_var(LRD,1, gripper_socket)", " rq_set_var(LBL,1, gripper_socket)", " rq_set_var(LGN,0, gripper_socket)", "end", "", "############################################", "# mm/inches functions", "############################################", "gripper_closed_norm = [100, 100, 100, 100]", "gripper_open_norm = [0, 0, 0, 0]", "gripper_closed_mm = [0, 0, 0, 0]", "gripper_open_mm = [50, 50, 50, 50]", "", "def rq_current_pos_mm(gripper_socket=1):", " pos_gripper = rq_current_pos(gripper_socket)", " pos_mm = gripper_to_mm(pos_gripper, gripper_socket)", " return round_value_2_dec(pos_mm)", "end", "", "def rq_current_pos_inches(gripper_socket=1):", " pos_gripper = rq_current_pos(gripper_socket)", " pos_mm = gripper_to_mm(pos_gripper, gripper_socket)", " pos_in = pos_mm / 25.4", " return round_value_2_dec(pos_in)", "end", "", "def rq_move_mm(pos_mm, gripper_socket=1):", " pos_gripper = mm_to_gripper(pos_mm, gripper_socket)", " rq_move(pos_gripper, gripper_socket)", "end", "", "def rq_move_and_wait_mm(pos_mm, gripper_socket=1):", " pos_gripper = mm_to_gripper(pos_mm, gripper_socket)", " rq_move_and_wait(pos_gripper, gripper_socket)", "end", "", "def rq_move_inches(pos_in, gripper_socket=1):", " pos_mm = pos_in * 25.4", " rq_move_mm(pos_mm, gripper_socket)", "end", "", "def rq_move_and_wait_inches(pos_in, gripper_socket=1):", " pos_mm = pos_in * 25.4", " rq_move_and_wait_mm(pos_mm, gripper_socket)", "end", "", "def get_closed_norm(gripper_socket):", " return gripper_closed_norm[gripper_socket - 1]", "end", "", "def get_open_norm(gripper_socket):", " return gripper_open_norm[gripper_socket - 1]", "end", "", "def get_closed_mm(gripper_socket):", " return gripper_closed_mm[gripper_socket - 1]", "end", "", "def get_open_mm(gripper_socket):", " return gripper_open_mm[gripper_socket - 1]", "end", "", "def set_closed_norm(closed_norm, gripper_socket):", " gripper_closed_norm[gripper_socket - 1] = closed_norm", "end", "", "def set_open_norm(open_norm, gripper_socket):", " gripper_open_norm[gripper_socket - 1] = open_norm", "end", "", "def set_closed_mm(closed_mm, gripper_socket):", " gripper_closed_mm[gripper_socket - 1] = closed_mm", "end", "", "def set_open_mm(open_mm, gripper_socket):", " gripper_open_mm[gripper_socket - 1] = open_mm", "end", "", "def gripper_to_mm(value_gripper, gripper_socket):", " closed_norm = get_closed_norm(gripper_socket)", " open_norm = get_open_norm(gripper_socket)", " closed_mm = get_closed_mm(gripper_socket)", " open_mm = get_open_mm(gripper_socket)", "", " value_norm = (value_gripper / 255) * 100", "", " slope = (closed_mm - open_mm) / (closed_norm - open_norm)", " value_mm = slope * (value_norm - closed_norm) + closed_mm", "", " if (value_mm > open_mm):", " value_mm_limited = open_mm", " elif (value_mm < closed_mm):", " value_mm_limited = closed_mm", " else:", " value_mm_limited = value_mm", " end", "", " return value_mm_limited", "end", "", "def mm_to_gripper(value_mm, gripper_socket):", " closed_norm = get_closed_norm(gripper_socket)", " open_norm = get_open_norm(gripper_socket)", " closed_mm = get_closed_mm(gripper_socket)", " open_mm = get_open_mm(gripper_socket)", "", " slope = (closed_norm - open_norm) / (closed_mm - open_mm)", " value_norm = (value_mm - closed_mm) * slope + closed_norm", "", " value_gripper = value_norm * 255 / 100", "", " if (value_gripper > 255):", " value_gripper_limited = 255", " elif (value_gripper < 0):", " value_gripper_limited = 0", " else:", " value_gripper_limited = round_value(value_gripper)", " end", "", " return value_gripper_limited", "end", "", "def round_value(value):", " value_mod = value % 1", "", " if(value_mod < 0.5):", " return floor(value)", " else:", " return ceil(value)", " end", "end", "", "def round_value_2_dec(value):", " value_x_100 = value * 100", " value_x_100_rounded = round_value(value_x_100)", " return value_x_100_rounded / 100", "end", "", "def clear_socket_buffer(gripper_socket=" + QUOTATION + "1" + QUOTATION + ", read_timeout = 0.1):", " byte_in_buffer = socket_read_byte_list(1, gripper_socket, read_timeout)", "", " while(byte_in_buffer[0] >= 1):", " byte_in_buffer = socket_read_byte_list(1, gripper_socket, read_timeout)", " end", "end", "", "def scale(value, rawRange, scaledRange):", " def computeSlope(inputRange, outputRange):", " outputRangeDelta = outputRange[1] - outputRange[0]", " inputRangeDelta = inputRange[1] - inputRange[0]", "", " if (inputRangeDelta == 0):", " return 0", " else:", " return outputRangeDelta / inputRangeDelta", " end", " end", "", " def computeIntercept(slope, inputRange, outputRange):", " return outputRange[0] - (slope * inputRange[0])", " end", "", " def clipScaledValue(outputScaledValue, outputRange):", " if (outputRange[0] < outputRange[1]):", " return clipWhenLowerLimitIsLessThanHigher(outputScaledValue, outputRange)", " else:", " return clipWhenLowerLimitIsGreaterThanHigherLimit(outputScaledValue, outputRange)", " end", " end", "", " def clipWhenLowerLimitIsGreaterThanHigherLimit(outputScaledValue, outputRange):", " if (outputScaledValue < outputRange[1]):", " return outputRange[1]", " elif (outputScaledValue > outputRange[0]):", " return outputRange[0]", " else:", " return outputScaledValue", " end", " end", "", " def clipWhenLowerLimitIsLessThanHigher(outputScaledValue, outputRange):", " if (outputScaledValue < outputRange[0]):", " return outputRange[0]", " elif (outputScaledValue > outputRange[1]):", " return outputRange[1]", " else:", " return outputScaledValue", " end", " end", "", " slope = computeSlope(rawRange, scaledRange)", " intercept = computeIntercept(slope, rawRange, scaledRange)", " scaledValue = slope * value + intercept", " return clipScaledValue(scaledValue, scaledRange)", "end", "", "def limit(value, range):", " return scale(value, range, range)", "end", "", "rq_obj_detect = 0", "#set_tool_voltage(24)", "#set_tool_communication(True, 115200, 0, 1, 1.5, 3.5)", "rq_wait_for_gripper_connected()", "rq_init_comm_if_connected(9, " + QUOTATION + "1" + QUOTATION + ")", "rq_init_comm_if_connected(2, " + QUOTATION + "2" + QUOTATION + ")", "rq_init_comm_if_connected(3, " + QUOTATION + "3" + QUOTATION + ")", "rq_init_comm_if_connected(4, " + QUOTATION + "4" + QUOTATION + ")", "rq_print_connected_grippers()", "connectivity_checked = [-1,-1,-1,-1]", "status_checked = [-1,-1,-1,-1]", "current_speed = [-1,-1,-1,-1]", "current_force = [-1,-1,-1,-1]", "set_closed_norm(100.0, 1)", "set_open_norm(0.0, 1)", "set_closed_mm(0.0, 1)", "set_open_mm(50.0, 1)", "set_closed_norm(100.0, 2)", "set_open_norm(0.0, 2)", "set_closed_mm(0.0, 2)", "set_open_mm(50.0, 2)", "set_closed_norm(100.0, 3)", "set_open_norm(0.0, 3)", "set_closed_mm(0.0, 3)", "set_open_mm(50.0, 3)", "set_closed_norm(100.0, 4)", "set_open_norm(0.0, 4)", "set_closed_mm(0.0, 4)", "set_open_mm(50.0, 4)", "", ])