'!TITLE "Keyence XG template"

#Define IPADDRESS "192.168.0.3" 	' IP address of KEYENCE XG
#Define SD_NO 1 		' SD card number to use
#Define PROG_NO 1 		' Inspection setting number to use
#Define CAL_NO 0		' CAL number of vis object to use

#Define VAL_INDEX 11	' Variable number to use
						' I[VAL_INDEX] : The number of work pieces detected
						' V[VAL_INDEX`] : Vision coordinate of work piece (x,y,angle)
						' P[VAL_INDEX`] : Robot coordinate of work piece

#Define HOME_INDEX 0	' P[HOME_INDEX] : Home position
#Define START_INDEX 1	' P[START_INDEX] : Start position
#Define DISC_INDEX 2	' P[DISC_INDEX] : Work piece placing position


Sub Main 
	
	' ------- Vision processing -------
	Call RunVision

	' ------- Calculate the robot travel distance -------
	Call TranceRobotPosition

	' ------- Robot motion -------
	Dim lCnt as Long	' Counter

	TakeArm Keep = 0

	' Move to the home position
	Move P, @P P[HOME_INDEX]

	For lCnt = 0 to I[VAL_INDEX] -1
	
		' Move to the start position
		Move P, @P P[START_INDEX]

		' ------- Robot motion at the picking position -------
		'Approach motion  Approach 100mm above of P[VAL_INDEX+lCnt]
		Approach P, P[VAL_INDEX+lCnt], @0 100
		' Descending motion    Move to  P[VAL_INDEX+lCnt]
		Move L, @0 P[VAL_INDEX+lCnt]
		' Write chuck/unchuck motion in this line, if necessary.
		Delay 500
		' Ascending motion    200mm
		Depart L, @0 200

		' ------- Robot motion at the work piece placing position -------
		'Approach motion  Approach 100mm above of P[DISC_INDEX]
		Approach P, P[DISC_INDEX], @0 100
		' Descending motion    Move to  P[DISC_INDEX]
		Move L, @0 P[DISC_INDEX] 
		' Write chuck/unchuck motion in this line, if necessary.
		Delay 500
		' Ascending motion    200mm
		Depart L, @0 200
	Next

	' Move to the start position
	Move P, @P P[START_INDEX]
	' Move to the home position
	Move P, @P P[HOME_INDEX]

	GiveArm

End Sub


' ------------------ Vision processing ------------------
Sub RunVision

	On Error Goto ErrProc		' Error process routine
	
	Dim caoXG As Object			' Variable for CaoProv
	Dim sResult As String		' S-type variable to store the result
	Dim vntRet As Variant		' V-type variable to store the result
	
	Dim vntValues As Variant	' Variable to analyze the result
	Dim lCnt As Long			' Counter
	
	' Set the number of detected work pieces to "0".
	I[VAL_INDEX] = 0

	' Connect KEYENCE XG
	caoXG = Cao.AddController("XG", "CaoProv.KEYENCE.VWXG", "", "Conn=eth:" & IPADDRESS)
	
	' Obtain the current inspection setting number
	vntRet = caoXG.ReadInspectSetting
	
	' Check the inspection setting number
	If ((vntRet(0) <> SD_NO) Or (vntRet(1) <> PROG_NO)) Then
		' Change the inspection setting number to the one you want to use.
		caoXG.ChangeInspectSetting SD_NO, PROG_NO
		' Wait until the inspection setting number change completes.
		Delay 1000 
	End If
	
	' Start measurement (all triggers) and obtain the result output
	sResult = caoXG.TriggerAndGetResult(-1)
	
	' Delimit the result with commas (",")
	vntValues = Split(sResult, ",")
	
	' Since the number of found work pieces is stored in vntValues(0),
	' substitute the value to I[VAL_INDEX].
	I[VAL_INDEX] = vntValues(0)
	
	' Substitute the work piece's X,Y, and Angle values.
	Dim lTemp As Long

	For lCnt = 0 To I[VAL_INDEX] -1
		lTemp = lCnt*3
		V[VAL_INDEX + lCnt] = V(vntValues(1+lTemp), vntValues(2+lTemp), vntValues(3+lTemp))
	Next

	Exit Sub

ErrProc:
	' If an error occurs, display the error code and end the program.
	PrintDbg "ErrorCode : " & Hex(Err.OriginalNumber)
	Cao.Controllers.Remove caoXG.Index
	caoXG = Nothing

End Sub


' ------------------ Calculate the robot travel distance ------------------
Sub TranceRobotPosition

	Dim lCnt As Long			' Counter
	Dim TargetVect As Vector	' Variable to store robot coordinates temporary
	
	For lCnt = 0 To I[VAL_INDEX] - 1
		' Convert work piece positions from vision coordinate to robot coordinate
		' by using vis object
		' (Calibration must be done with vis object beforehand)
		TargetVect = Vis.Trans(CAL_NO, PosX(V[VAL_INDEX+lCnt]), PosY(V[VAL_INDEX+lCnt]))
		
		' Substitute values to P-type variable.
		' Posture: Current posture, Fig = -2
		P[VAL_INDEX+lCnt] = CurPos
		LetP P[VAL_INDEX+lCnt] = TargetVect
		LetRz P[VAL_INDEX+lCnt] = PosZ(V[VAL_INDEX+lCnt])
		LetF P[VAL_INDEX+lCnt] = -2
	Next
	 
End Sub