Fanuc 机器人二次开发（Socket通讯加数据传输）

        在开发Yaskawa机器人之前，同样开发过Fanuc机器人，在我接触Fanuc机器人的时候，我还吐槽那老式的示教器极其难用，设置也不是特别合理，直到碰到了Yaskawa。过段时间，又得去摸Nachi机器人，适配这个工作既不能了解比较深的学问，又得时常去摸别的品牌的机器人，被一些难搞的设计整的头皮发麻，最重要的还是很无聊，适配的经验就是先摸清楚程序怎么运行，怎么能去控制机器人，再考虑逻辑怎么写，测试环境也要考察一下，才能顺利进行下去。

        言归正传，Fanac机器人要想在后台时刻通讯，必须要把karel设置功能开启，并将程序运行设置成电源开启后自动运行。机器人的选项包可以在ROBOGUIDE仿真里面看一下，网络通讯使用的是socket选项包，并且要把系统变量的karel enable修改一下。测试时，把karel编译的.pc程序插U盘导入在实际的机器人上进行运行。Fanuc好在不需要像Yaskawa一样只能实机测试软件，在没有机器人实机的条件下，可以在仿真环境下运行。但是Karel程序不能主动的控制机器人运动，需要编写额外的示教程序，然后在Karel程序里面进行调用，我之前的项目是通过通信，将机器人当时的位姿记录下来，记录完成后，自动的按照传输的点位进行运动。

        Fanuc机器人的控制组很容易被程序占用，如果是不进行控制的程序，记得在程序详细里把控制组修改成全都是0。为了保密，这里只分享一点通用的代码，包括Socket通信、获取当前的姿态信息、数据打包、数据转换、发送。如果想自动化运行的话，可以写个另外的控制程序，在后台运行通信程序的时候，再运行另外一个karel控制程序来调用运动的TP程序。通信程序将想移动到的点位、运动参数等都写在寄存器里，TP程序读取寄存器进行运动，而两个karel程序之间可以采用CMOS变量进行通讯。在karel设置里可以同时运行两个karel程序，只要保证控制组不冲突，并且忽略报警跟报错即可，即本资源代码的前几行，把注释删掉即可。

PROGRAM TCP_TEST
%NOLOCKGROUP

--%NOPAUSE  = ERROR + TPENABLE + COMMAND
--%NOABORT = ERROR + TPENABLE + COMMAND

CONST 
    ROBOT_STATE_SIZE = 8      --size of robot state
    data_package_length = 50  --number of data packet byte


VAR
    g_move_target_data  IN CMOS FROM Main_TEST: ARRAY[12] OF REAL  --robot move parameter
    g_move_parmas_data  IN CMOS FROM Main_TEST: ARRAY[2] OF INTEGER   --robot type parameter
    order_head : ARRAY[4] OF BYTE                                        --order hedd of data packet
    loop_connect :  BOOLEAN                                              --flag of TCP Connect 
    fileVar  : FILE                                                      --recieve and send data by this file 
    data_buffer : ARRAY[data_package_length] OF BYTE                     --byte type of send data
    send_data : ARRAY[data_package_length] OF STRING[1]                  --send data transform to ASCⅡ
    recv_data : ARRAY[data_package_length] OF STRING[1]                  --recieve data from host software 
    cur_time : STRING[20]                                                --current time
    i,n,j,m : INTEGER                                                    --iterator
    status : INTEGER   
    loop : BOOLEAN                                                        --itrator in routine
CONST
    TCPSever = 'S1:' --sever port of system par
    
TYPE 
    RoboTarget = STRUCTURE
        current_joint : JOINTPOS
        current_pose : XYZWPR
    ENDSTRUCTURE

-------------------------------------------------------------------------  
ROUTINE VariableInit FROM TCP_TEST                                  --*√*state Init
ROUTINE InterruptInit FROM TCP_TEST                                --*√*get home_detect state
ROUTINE TcpInit FROM TCP_TEST                                      --*√*create tcp connect with a cilent 
ROUTINE Analyze FROM TCP_TEST                                      --analyze data packet from host-software
ROUTINE SendStateData FROM TCP_TEST                                  --collect and transform state data for send
ROUTINE Oxtofloat(data:ARRAY[*] OF STRING) : REAL FROM  TCP_TEST    --*√*transform Ox to float. input 4 byte = ARRAY[8] OF STRING[1], output: float
ROUTINE Oxtoint(data:ARRAY[*] OF STRING) : INTEGER FROM TCP_TEST     --*√*transform Ox to int. input 1 byte = ARRAY[2] OF STRING[1], output: INTEGER   
ROUTINE GET_CUR_TIME : STRING FROM TCP_TEST                         --*√*get current time. output: string
ROUTINE inttoOx(data : INTEGER) : ARRAY[*] OF STRING FROM TCP_TEST   --*√*transform int to Ox. input: INTEGER, output 1 byte = ARRAY[2] OF STRING[1] 
ROUTINE send_TCP(data:ARRAY[*] OF STRING) FROM TCP_TEST             --*√*operation of send data packet to cilent. input: STRING[96] 
ROUTINE SplitData FROM TCP_TEST                                     --Split data packet to per-type
ROUTINE GET_ACTRO_SPEED : REAL FROM TCP_TEST                        --*√*get current robot tcp speed
-------------------------------------------------------------------------  

BEGIN
    WRITE(CHR(128),CHR(137)) -- clear TP user window
    VariableInit
    TcpInit
    Analyze

END TCP_TEST

------------------------------------------------------------------------- 
--Step1.2.2: local var variable declaration
--Init Robot State

ROUTINE VariableInit 
VAR 
	entry : INTEGER
BEGIN
    g_analyze_complete = FALSE 
    SET_VAR(entry, '*SYSTEM*','$SCR_GRP[1].$M_POS_ENB',TRUE,status)
    SET_VAR(entry, '*SYSTEM*','$SCR_GRP[1].$M_DST_ENB',TRUE,status) --set param for current speed
    $GROUP[1].$UTOOL = $MNUTOOL[1,1]                                --uTool 1  set param for get cur pos
	$GROUP[1].$UFRAME = $MNUFRAME[1,1]                              --uFrame 1
END VariableInit

------------------------------------------------------------------------- 
--Init Tcp Socket 
ROUTINE TcpInit
VAR 
    status : INTEGER
    dataType : INTEGER
    entry : INTEGER
CONST 
    --IP = '192.168.1.1'
    SeverPort = 8800
BEGIN 
    dataType = 3 --1 : INTEGER   2 : REAL  3 : STRING
    SET_FILE_ATR(fileVar,ATR_IA)
    SET_VAR(entry, '*SYSTEM*','$HOSTS_CFG[1].$SERVER_PORT',SeverPort,status)
    MSG_DISCO(TCPSever,status)
    WRITE('~robot start listening~',CR)
    MSG_CONNECT(TCPSever,status)
    IF status = 0 THEN
        WRITE('TCP connect successful',CR)
    ENDIF
    WRITE ('Robot start read file',CR)
    OPEN FILE fileVar('RW',TCPSever)
    status =  IO_STATUS(fileVar)
    IF status <> 0 THEN 
        WRITE('file open fail',CR)
    ENDIF
END TcpInit

------------------------------------------------------------------------- 
--read data packet and analyze
ROUTINE Analyze 
VAR
    order_type : INTEGER
    entry : INTEGER
    status : INTEGER
    temp_str : STRING[100]
   
BEGIN 
	loop = TRUE
	WHILE loop DO	
	    BYTES_AHEAD(fileVar, entry, status)
	    DELAY 50
	    IF status <> 0  THEN 
	        WRITE('return entry-data fail',CR)
	    ELSE
	        IF entry = 50 THEN   --READ 50 CHAR
	            READ fileVar (temp_str :: 50)
	            status = IO_STATUS(fileVar)
	            IF status <> 0 THEN
	                WRITE('read recvfile fail',CR)
	            ELSE 
	                FOR i = 1 TO 50 DO
	                recv_data[i] = SUB_STR(temp_str,i,1) 
	                WRITE(recv_data[i])
	                ENDFOR
	                SplitData --处理数据
	                SendStateData --发送数据
	                WRITE('recv error type data',CR)
	                ENDSELECT
	            ENDIF
	         ENDIF
	    ENDIF
	ENDWHILE
END Analyze


 ------------------------------------------------------------------------- 
--send datapcket to host software
ROUTINE SendStateData
CONST
    JOINT_SIZE = 6
    POSE_SIZE = 6
    MOVE_PARAM = 2
VAR
    Current_data : RoboTarget
    Dealy_data : RoboTarget
    Joint_array : ARRAY[9] OF REAL
    joint_data : ARRAY[JOINT_SIZE] OF REAL
    pose_data : ARRAY[POSE_SIZE] OF REAL  
    temp_byte : ARRAY[8] OF BYTE
    status :INTEGER
    act_speed : REAL
    move_type : INTEGER --control robot move type
    temp_int : INTEGER
BEGIN

    Current_data.current_joint = CURJPOS(0,0)
    Current_data.current_pose = CURPOS(0,0)
    CNV_JPOS_REL(Current_data.current_joint,Joint_array,status)
    FOR i = 1 TO 6 DO
        joint_data[i] = Joint_array[i] --获取当前角度
    ENDFOR
    pose_data[1] = Current_data.current_pose.w --获取当前基于基座的法兰偏移值
    pose_data[2] = Current_data.current_pose.p
    pose_data[3] = Current_data.current_pose.r
    pose_data[4] = Current_data.current_pose.x
    pose_data[5] = Current_data.current_pose.y
    pose_data[6] = Current_data.current_pose.z

    act_speed = GET_ACTRO_SPEED --获取当前速度

    --joint_data    
    j = 1
    FOR i = 1 TO JOINT_SIZE DO
        ADD_REALPC(temp_byte,1,joint_data[i],status)
        data_buffer[(4*j-3)] = temp_byte[3]
        data_buffer[(4*j-2)] = temp_byte[4]
        data_buffer[(4*j-1)] = temp_byte[5]
        data_buffer[(4*j)] = temp_byte[6]
        j = j + 1
    ENDFOR 

    --pose_data
    j = 1
    FOR i = 1 TO POSE_SIZE DO 
        ADD_REALPC(temp_byte,1,pose_data[i],status)
        data_buffer[(24+4*j-3)] = temp_byte[3]
        data_buffer[(24+4*j-2)] = temp_byte[4]
        data_buffer[(24+4*j-1)] = temp_byte[5]
        data_buffer[(24+4*j)] = temp_byte[6]
        j = j + 1 
    ENDFOR
    data_buffer[49] = act_speed
    data_buffer[50] = move_type
    --transform byte data to ASCⅡstring
    FOR i = 1 TO 50 DO
    	temp_int = data_buffer[i]    --将浮点数转换成十六进制的字节
        send_data[i] = CHR(temp_int) --转换成字符
    ENDFOR
    --send send_data by socket
    send_TCP(send_data)
END SendStateData

------------------------------------------------------------------------- 
--send data use tcp connect
ROUTINE send_TCP
VAR
    status : INTEGER
BEGIN
    WRITE fileVar(data)
    status  = IO_STATUS(fileVar)
    IF status = 0 THEN
        WRITE('send data successful',CR)
    ENDIF
END send_TCP


-------------------------------------------------------------------------
--get current time 
ROUTINE GET_CUR_TIME
VAR
    CURTIME : INTEGER
    DATA_TIME : STRING[50]
BEGIN
    GET_TIME(CURTIME)
    CNV_TIME_STR(CURTIME,DATA_TIME)
    RETURN (DATA_TIME)
END GET_CUR_TIME

-------------------------------------------------------------------------
--spilt data packet and 
ROUTINE SplitData
VAR
    --ASCⅡ
    joint_str : ARRAY[48] OF STRING[1]
    pose_str : ARRAY[48] OF STRING[1]
    MoveTarget_str : ARRAY[48] OF INTEGER --FLOAT   4BYTE/PER
    MoveParams_str : ARRAY[2] OF INTEGER  --INT     1BYTE/PER
    temp_real : ARRAY[8] OF REAL
    inttoOx_str : ARRAY[2] OF STRING[1]
    temp_str : ARRAY[8] OF STRING[1]
    temp_int : INTEGER
BEGIN
	WRITE(CR)
    FOR i = 1 TO 48 DO
        MoveTarget_str[i] = ORD(recv_data[i+4],1)
    ENDFOR
    FOR i = 1 TO 2 DO 
        MoveParams_str[i] = ORD(recv_data[i + 48],1)  --字符转换成十六进制的byte
    ENDFOR

    FOR i = 1 TO 24 DO
    	temp_int = MoveTarget_str[i]
    	inttoOx_str = inttoOx(temp_int) --Byte整数转换成十六进制字符
        joint_str[(2*i-1)] = inttoOx_str[1] 
        joint_str[(2*i)] = inttoOx_str[2]
        WRITE(CR,joint_str[1],joint_str[2])  
    ENDFOR

    FOR i = 25 TO 48 DO
    	temp_int = MoveTarget_str[i]
    	inttoOx_str = inttoOx(temp_int)
        pose_str[(2*i-49)] = inttoOx_str[1]
        pose_str[(2*i-48)] = inttoOx_str[2]
    ENDFOR
    ------------------------------------------------
    FOR i = 1 TO 4 DO
        recv_head[i] = OrderHead_str[i]
    ENDFOR
    
    FOR i = 1 TO 6 DO
    	FOR j = 1 TO 8 DO
	        temp_str[j] = joint_str[i*8-8+j]
    	ENDFOR
    		temp_real[i] = Oxtofloat(temp_str) --十六进制转换成浮点数
	        g_move_target_data[i] = temp_real[i]
    ENDFOR 

    FOR i = 1 TO 6 DO
    	FOR j = 1 TO 8 DO
        temp_str[j] = pose_str[i*8-8+j]
    	ENDFOR
    	temp_real[i] = Oxtofloat(temp_str)
        g_move_target_data[i+6] = temp_real[i]
    ENDFOR 

    FOR i = 1 TO 2 DO
        g_move_parmas_data[i] = MoveParams_str[i]
    ENDFOR
END SplitData

------------------------------------------------------------------------- 
--TRANSFORM Ox to float
ROUTINE Oxtofloat
VAR 
    B : REAL
    data_binary : ARRAY[32] OF INTEGER
    data_ox : ARRAY[8] OF INTEGER
    data_float : REAL 	
    str_array : ARRAY[6] OF STRING[1] 
    S : INTEGER
    E : INTEGER
    int_exe : INTEGER
    real_exe : REAL
    i,j,n,m : INTEGER
    str : STRING[1]
    
BEGIN
    str_array[1] = 'A'
    str_array[2] = 'B'
    str_array[3] = 'C'
    str_array[4] = 'D'
    str_array[5] = 'E'
    str_array[6] = 'F'
    
    FOR i = 1 TO 8 DO
        j = 0
        m = 0
        str = data[i]
        FOR n = 1 TO 6 DO
            IF str = str_array[n] THEN
                m = n
            ENDIF
        ENDFOR
        SELECT m OF 
            CASE (1) : 
                j = 10
            CASE (2) : 
                j = 11
            CASE (3) : 
                j = 12
            CASE (4) : 
                j = 13
            CASE (5) : 
                j = 14
            CASE (6) : 
                j = 15
        ELSE: 
            CNV_STR_INT(str,j)
        ENDSELECT
        data_ox[i] = j
    ENDFOR
    
    FOR i = 1 TO 4 DO
        j = data_ox[(2*i-1)]
        n = data_ox[(2*i)]
        IF j >= 8 THEN 
            data_binary[(i*8-7)] = 1
        j = j - 8
        ELSE 
            data_binary[(i*8-7)] = 0
        ENDIF
        IF j >= 4 THEN 
            data_binary[(i*8-6)] = 1
            j = j - 4
        ELSE 
            data_binary[(i*8-6)] = 0
        ENDIF
        IF j >= 2 THEN 
            data_binary[(i*8-5)] = 1
            j =j - 2
        ELSE 
            data_binary[(i*8-5)] = 0
        ENDIF
        IF j >= 1 THEN 
            data_binary[(i*8-4)] = 1
            j = j - 1
        ELSE 
            data_binary[(i*8-4)] = 0
        ENDIF
        
        IF n >= 8 THEN 
            data_binary[(i*8-3)] = 1
        n = n - 8
        ELSE 
            data_binary[(i*8-3)] = 0
        ENDIF
        IF n >= 4 THEN 
            data_binary[(i*8-2)] = 1
            n = n - 4
        ELSE 
            data_binary[(i*8-2)] = 0
        ENDIF
        IF n >= 2 THEN 
            data_binary[(i*8-1)] = 1
            n = n - 2
        ELSE 
            data_binary[(i*8-1)] = 0
        ENDIF
        IF n >= 1 THEN 
            data_binary[(i*8)] = 1
            j = j - 1
        ELSE 
            data_binary[(i*8)] = 0
        ENDIF
    ENDFOR

    IF data_binary[1] = 0 THEN
            S = 1
    ELSE 
            S = -1
    ENDIF

    E = 0
    int_exe = 1
    FOR i = 9 DOWNTO 2 DO 
        E = E + data_binary[i] * int_exe
        int_exe = int_exe * 2
    ENDFOR
    E = E - 127
    real_exe = 0.5
    FOR i = 1 TO E+1 DO
        real_exe = real_exe * 2
    ENDFOR	
    data_float = 1 * real_exe
    FOR i = 10 DOWNTO 32 DO 
        real_exe = real_exe * 0.5
        data_float = data_binary[i] * real_exe  + data_float 
    ENDFOR
    data_float = data_float * S
    RETURN (data_float)
END Oxtofloat

------------------------------------------------------------------------- 
--int transform to Ox
ROUTINE inttoOx
VAR 
    result : ARRAY[2] OF STRING[1] 
    temp_str : ARRAY[2] OF STRING[2]
    str_array : ARRAY[6] OF STRING[1] 
    
BEGIN 
    str_array[1] = 'A'
    str_array[2] = 'B'
    str_array[3] = 'C'
    str_array[4] = 'D'
    str_array[5] = 'E'
    str_array[6] = 'F'
    IF data DIV 16 < 10 THEN 
        CNV_INT_STR(data DIV 16,2,0,temp_str[1])
        result[1] = SUB_STR(temp_str[1],2,1)
    ELSE    
        FOR i = 1 TO 6 DO
            IF data DIV 16 = 9 + i THEN 
                result[1] = str_array[i]
            ENDIF
        ENDFOR
    ENDIF
    IF data MOD 16 < 10 THEN 
        CNV_INT_STR(data MOD 16,2,0,temp_str[2])
        result[2] = SUB_STR(temp_str[2],2,1)
    ELSE
        FOR i = 1 TO 6 DO
            IF data MOD 16 = 9 + i THEN
                result[2] = str_array[i]
            ENDIF
        ENDFOR
    ENDIF
    RETURN (result)
END inttoOx
------------------------------------------------------------------------- 
ROUTINE GET_ACTRO_SPEED 

VAR
    speed : REAL
    entry : INTEGER
BEGIN
    speed = $SCR_GRP[1].$MCH_SPD
    RETURN(speed)
END GET_ACTRO_SPEED