Nachi机器人TCP通讯
输入格式211,-265,167\n
逗号为英文格式，\n表示坐标输入后记得回车
NachiRobot
VARIABLE.INC
' Variable declarations
' V: global variable
' L: local variable
' IP & port
IP_LAST,217
PORT,10030
' %: integer variable
ERR_COM,V%[1]
ERRCODE1,V%[2]
ERRCODE2,V%[3]
SENT_BYTES,V%[11]
RECIEVED_BYTES,V%[12]
BYTE_DATA,V%[13]
TOSEND_BYTES,V%[14]
rCurrentNum,V%[15]
DECISION_VALUE,V%[16]
RECVBUF_READ_READY,V%[21]
RECVBUF_WRITE_READY,V%[22]
SENDBUF_READ_READY,V%[23]
SENDBUF_WRITE_READY,V%[24]
SHUT_DOWN_EXEC,V%[25]
SHUT_DOWN_FINISHED,V%[26]
' $: str variable
SEND_STR,V$[1]
CharString,V$[2]
ARG_STR,V$[3]
STR_SAVE_OFFSET,10
RECV_STR1,V$[11]
RECV_STR2,V$[12]
RECV_STR3,V$[13]
RECV_STR4,V$[14]
RECV_STR5,V$[15]
RECV_STR6,V$[16]
RECV_STR7,V$[17]
' !: REAL Number
MOVE_POS_X,V![11]
MOVE_POS_Y,V![12]
MOVE_POS_Z,V![13]
MOVE_POS_RZ,V![14]
MOVE_POS_RY,V![15]
MOVE_POS_RX,V![16]
MZ04-01-A.999
INCLUDE "VARIABLE"
' 初始化
RECVBUF_READ_READY = 0
RECVBUF_WRITE_READY = 0
SENDBUF_READ_READY = 0
SENDBUF_WRITE_READY = 0
SHUT_DOWN_EXEC = 0
SHUT_DOWN_FINISHED = 0
ERR_COM = 0
MOVE_POS_RX = 0
MOVE_POS_RY = 90
MOVE_POS_RZ = 0
' Fixture direction fixing
' connect open --USERTASK.999
FORKMCR 999, 10000
*READY
' send welcome commond
CallProc WaitForSendReady()
SEND_STR = "Please enter coordinates or exit:"
CallProc ExecuteSend()
CallProc ExecuteRecieve()
IF "exit" = RECV_STR1 THEN *FINAL
' recv value
' CallProc ExecuteRecieve()
' move
DECISION_VALUE = VAL(RECV_STR4)
IF DECISION_VALUE = 0 AND rCurrentNum = 3
MOVE_POS_X = VAL(RECV_STR1)
MOVE_POS_Y = VAL(RECV_STR2)
MOVE_POS_Z = VAL(RECV_STR3)
' MOVE_POS_RX = VAL(RECV_STR4)
' MOVE_POS_RY = VAL(RECV_STR5)
' MOVE_POS_RZ = VAL(RECV_STR6)
CallProc ExecuteMove()
ENDIF
GOTO *READY
' program finished
*FINAL
SHUT_DOWN_EXEC = 1
IF SHUT_DOWN_FINISHED = 0 THEN *FINAL
END
' *********subfunction*********
UsrProc WaitForSendReady()
*SEND_LOOP
IF ERR_COM <> 0 THEN *FINAL
IF SENDBUF_WRITE_READY = 0 THEN *SEND_LOOP
EndProc
UsrProc ExecuteSend()
SENDBUF_READ_READY = 1
EndProc
UsrProc ExecuteRecieve()
RECVBUF_WRITE_READY = 1
*RECV_LOOP
IF ERR_COM <> 0 THEN *FINAL
IF RECVBUF_READ_READY = 0 THEN *RECV_LOOP
RECVBUF_READ_READY = 0
EndProc
UsrProc ExecuteMove()
MOVEX A=1,AC=3,SM=3, M1X, L, (MOVE_POS_X, MOVE_POS_Y, MOVE_POS_Z, MOVE_POS_RX,MOVE_POS_RY,MOVE_POS_RZ),R=10,MS
EndProc
USERTASK-A.999
' TCP/IP Server
INCLUDE "VARIABLE"
*CREATE
FOR L100%=0 TO 20 STEP 1
SOCKCREATE 1, 0 'Socket#1 & TCP/IP
IF E1%>=0 THEN *SOCKBIND
PAUSE 500
NEXT
GOTO *ERROR_END
*SOCKBIND
SOCKBIND 1, PORT ' PORT = 10030
IF E%[1] < 0 THEN *ERROR_END
' wait connect
*SOCKWAIT
SOCKWAIT 1,2,0
IF E%[1] < 0 THEN *ERROR_END
' ----------------------------
*COM_LOOP
SENDBUF_WRITE_READY = 1
IF SENDBUF_READ_READY <> 0 THEN *SEND_BUF
IF RECVBUF_WRITE_READY <> 0 THEN *RECV_BUF
IF SHUT_DOWN_EXEC <> 0 THEN *NORMAL_END
GOTO *COM_LOOP
*RECV_BUF
RECVBUF_WRITE_READY = 0
' receive data--------
rCurrentNum = 1
V$[rCurrentNum + STR_SAVE_OFFSET] = ""
' RECV_STR1(V11$) = ""
*RECV_BYTE_LOOP
SOCKRECV 2, 1, 1, 0, RECIEVED_BYTES
' write to RECIEVED_BYTES(V12%)
IF E%[1] < 0 THEN *ERROR_END
' Extracts 1 byte of data from the buffer
GETBYTE 1, BYTE_DATA, 0
CharString = CHR$(BYTE_DATA)
IF 10 = BYTE_DATA OR 13 = BYTE_DATA
' In ASCII code 10 is line feed and 13 is carriage return
' (their as the end of an instruction)
GOTO *RECV_END ' Out1
ENDIF
IF "," = CharString
rCurrentNum = rCurrentNum + 1
V$[rCurrentNum + STR_SAVE_OFFSET] = ""
GOTO *RECV_BYTE_LOOP
ENDIF
V$[rCurrentNum + STR_SAVE_OFFSET] = V$[rCurrentNum + STR_SAVE_OFFSET] + CharString
GOTO *RECV_BYTE_LOOP
' receive data---------
*RECV_END
SOCKRECV 2, 2, 1, 0, RECIEVED_BYTES
' write to RECIEVED_BYTES(V12%)
IF E%[1] < 0 THEN *ERROR_END
RECVBUF_READ_READY = 1
' ExecuteRecieve() Loop
' BYTE_DATA = 0
GOTO *COM_LOOP
' receive data end---------
' sent data--------
*SEND_BUF
SENDBUF_READ_READY = 0
SOCKSENDSTR 2, SEND_STR, LEN(SEND_STR), 0, SENT_BYTES, 2
IF E%[1] < 0 THEN *ERROR_END
GOTO *COM_LOOP
' sent data end--------
*ERROR_END
ERRCODE1 = E%[1]
ERRCODE2 = E%[2]
ERR_COM = 1
' close the connect
*NORMAL_END
SOCKCLOSE 1
SOCKCLOSE 2
SHUT_DOWN_FINISHED = 1
END