&ACCESS RVP
DEF serial_to_robot( )
;This program waits for a valid position being received at the COM2 interface and then moves the robot to that position.

;--------- Declaration ---------
DECL INT HANDLE, I, J, CREAD_OFFSET, ANSWER, NUMBER
DECL CHAR NOTIFY_TEXT[5], READ_STRING[20]
DECL REAL TIMEOUT, CHECKSUM, SUMCOORD
DECL STATE_T CWRITE_STATE, CREAD_STATE
DECL MODUS_T MODUS
DECL AXIS HOME
DECL POS PositionRobot
EXT BAS(BAS_COMMAND:IN, REAL:IN)

;---------- Initialization ---------
BAS(#INITMOV,0)    ;Initialization of speed, acceleration and coordinate system
$BASE = $WORLD     ;set the base coordinate system (mandatory when using a frame)
$TOOL = $NULLFRAME ;set not to use any tool coordinate system
HOME={AXIS: A1 0,A2 -90,A3 90,A4 0,A5 0,A6 0}  ;set home position
CHECKSUM=0
SUMCOORD=0
I=1

FOR J = 1 to 6
 $VEL_AXIS[J]=30  ;x% max. speed
 $ACC_AXIS[J]=100 ;x% max. acceleration
ENDFOR

;---------------------------------------
;              Main program 
;---------------------------------------

;------------- Open channel to COM2 -----------
COPEN(:SER_2,HANDLE) ;Open a channel to COM2 according to config in serial.ini. If XON/XOFF is used, then a XON symbol is sent by COPEN.

IF HANDLE==0 THEN ;If an error occured during opening the channel, then stop the program.
 HALT
ENDIF

MODUS=#SYNC 

LOOP ;endless loop

 ;------------- Write to COM2 -----------
 CWRITE(HANDLE,CWRITE_STATE,MODUS," ") ;Send a space to mark the start of the loop.
 CWRITE(HANDLE,CWRITE_STATE,MODUS,"%d",I)  ;Send counter value to track the loop.

 ;------------ Read from COM2 -----------
 TIMEOUT=60.0
 CREAD_OFFSET=0
 MODUS=#ABS

 CREAD(HANDLE,CREAD_STATE,MODUS,TIMEOUT,CREAD_OFFSET,"%f %f %f %f %f %f %d %d %f",PositionRobot.X,PositionRobot.Y,PositionRobot.Z,PositionRobot.A,PositionRobot.B,PositionRobot.C,PositionRobot.S,PositionRobot.T,CHECKSUM)
 ;Send for example the string "2000 -800 1800 -90 90 -90 2 10 2922" to COM2. THe last value is the sum of all previous values.

 IF CREAD_STATE.RET1==#CMD_TIMEOUT THEN ;If the timeout time has been exceeded, then exit the loop. 
  EXIT
 ENDIF

 NOTIFY_TEXT[] = "     " ;clear variable
 NOTIFY_TEXT[] = "X: "
 NOTIFY_REAL(NOTIFY_TEXT[],PositionRobot.X) ;NOTIFY_REAL is a subprogram defined below
 NOTIFY_TEXT[] = "Y: "
 NOTIFY_REAL(NOTIFY_TEXT[],PositionRobot.Y)
 NOTIFY_TEXT[] = "Z: "
 NOTIFY_REAL(NOTIFY_TEXT[],PositionRobot.Z)
 NOTIFY_TEXT[] = "A: "
 NOTIFY_REAL(NOTIFY_TEXT[],PositionRobot.A)
 NOTIFY_TEXT[] = "B: "
 NOTIFY_REAL(NOTIFY_TEXT[],PositionRobot.B)
 NOTIFY_TEXT[] = "C: "
 NOTIFY_REAL(NOTIFY_TEXT[],PositionRobot.C)
 NOTIFY_TEXT[] = "S: "
 NOTIFY_INT(NOTIFY_TEXT[],PositionRobot.S)
 NOTIFY_TEXT[] = "T: "
 NOTIFY_INT(NOTIFY_TEXT[],PositionRobot.T)
 NOTIFY_TEXT[] = "CHK: "
 NOTIFY_REAL(NOTIFY_TEXT[],CHECKSUM)
 SUMCOORD=PositionRobot.X+PositionRobot.Y+PositionRobot.Z+PositionRobot.A+PositionRobot.B+PositionRobot.C+PositionRobot.S+PositionRobot.T
 NOTIFY_TEXT[] = "SUM: "
 NOTIFY_REAL(NOTIFY_TEXT[],SUMCOORD)

 IF PositionRobot.X==0 THEN ;Send the string "0" to COM2 to exit the loop.
  EXIT
 ENDIF

 ;---- Move robot to the read position ------
 WAIT FOR $IN[16]==TRUE ;wait for an impulse on input 16 to trigger continuation  

 IF SUMCOORD==CHECKSUM THEN
  PTP PositionRobot
 ENDIF

 I=I+1
 CWRITE(HANDLE,CWRITE_STATE,MODUS,".") ;Send a dot to mark the end of the loop.

ENDLOOP

WAIT FOR $IN[16]==TRUE ;wait for an impulse on input 16 to trigger continuation  
PTP HOME
CCLOSE(HANDLE,CWRITE_STATE) ;info: a XOFF is sent after closing

END


;---------------------------------------
;              Sub programs 
;---------------------------------------

;subprogram to display the value of a real variable
DEF NOTIFY_REAL(NOTIFY__TEXT[]:OUT,NUMBR:IN) ;char arrays must be called by reference (OUT)

 DECL CHAR CHARACTR, NOTIFY__TEXT[] ;index must be omitted here 
 DECL REAL NUMBR
 DECL INT MSG_OFFSET
 DECL MSG_T EMPTY_MSG
 DECL STATE_T MSG_STATE

 EMPTY_MSG={MSG_T: VALID FALSE,RELEASE FALSE,TYP #NOTIFY,MODUL[] " ",KEY[] " ",PARAM_TYP #VALUE,PARAM[] " ",DLG_FORMAT[] " ",ANSWER 0}
 $MSG_T=EMPTY_MSG
 MSG_OFFSET=0
 SWRITE($MSG_T.KEY[],MSG_STATE,MSG_OFFSET,NOTIFY__TEXT[]) ;write MSG_OFFSET into $MSG_T.KEY[]
 SWRITE($MSG_T.KEY[],MSG_STATE,MSG_OFFSET,"%f",NUMBR) ;add the value of the real variable at the end of $MSG_T.KEY[]
 $MSG_T.TYP=#NOTIFY ;set message typ
 WAIT SEC 0.2 ;mandatory to solve a bug with unreleased status messages
 $MSG_T.VALID=TRUE
 WAIT SEC 0.2

END

;subprogram to display the value of an integer variable
DEF NOTIFY_INT(NOTIFY__TEXT[]:OUT,NUMBR:IN) ;char arrays must be called by reference (OUT)

 DECL CHAR NOTIFY__TEXT[] ;index must be omitted here 
 DECL INT NUMBR, MSG_OFFSET
 DECL MSG_T EMPTY_MSG
 DECL STATE_T MSG_STATE

 EMPTY_MSG={MSG_T: VALID FALSE,RELEASE FALSE,TYP #NOTIFY,MODUL[] " ",KEY[] " ",PARAM_TYP #VALUE,PARAM[] " ",DLG_FORMAT[] " ",ANSWER 0}
 $MSG_T=EMPTY_MSG
 MSG_OFFSET=0
 SWRITE($MSG_T.KEY[],MSG_STATE,MSG_OFFSET,NOTIFY__TEXT[]) ;write MSG_OFFSET into $MSG_T.KEY[]
 SWRITE($MSG_T.KEY[],MSG_STATE,MSG_OFFSET,"%d",NUMBR) ;add NUMBR at the end of $MSG_T.KEY[]
 $MSG_T.TYP=#NOTIFY ;set message typ
 WAIT SEC 0.2 ;mandatory to solve a bug with unreleased status messages
 $MSG_T.VALID=TRUE
 WAIT SEC 0.2

END

;subprogram to display a string
DEF NOTIFY_STR(NOTIFY__TEXT[]:OUT, READ_STR[]:OUT) ;char arrays must be called by reference (OUT)

 DECL CHAR NOTIFY__TEXT[],READ_STR[] ;index must be omitted here 
 DECL INT MSG_OFFSET
 DECL MSG_T EMPTY_MSG
 DECL STATE_T MSG_STATE

 EMPTY_MSG={MSG_T: VALID FALSE,RELEASE FALSE,TYP #NOTIFY,MODUL[] " ",KEY[] " ",PARAM_TYP #VALUE,PARAM[] " ",DLG_FORMAT[] " ",ANSWER 0}
 $MSG_T=EMPTY_MSG
 MSG_OFFSET=0
 SWRITE($MSG_T.KEY[],MSG_STATE,MSG_OFFSET,NOTIFY__TEXT[]) ;write MSG_OFFSET into $MSG_T.KEY[]
 SWRITE($MSG_T.KEY[],MSG_STATE,MSG_OFFSET,READ_STR[]) ;add CHARACTR at the end of $MSG_T.KEY[]
 $MSG_T.TYP=#NOTIFY ;set message typ
 WAIT SEC 0.2 ;mandatory to solve a bug with unreleased status messages
 $MSG_T.VALID=TRUE
 WAIT SEC 0.2

END