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