HandEyeConvPos

Example

'!TITLE "HandEyeEVP"

' Approach distance 	100mm
' Depart distance	100mm

' Robot waiting position	P[RobotImageAquisitionNumber]
'                           (Save the robot posture at the EVP project creation.
'                            Also, check if the camera is vertical to the workpiece.)
' Position to place a workpiece       P[PlaceNumber]

' With EVPRun2, take snapshots, process image data,
'               read  workpiece position, and then set these values into the following data.
' The number of detected workpieces  I[NumberOfWorks]
' Position of detected workpieces    P[WorkPlaceNumber], P[WorkPlaceNumber +1], ... ,
'                                    P[WorkPlaceNumber + the number of detected workpieces -1]
' The type of detected workpieces    I[WorkTypeNumber], I[WorkTypeNumber +1], ... , 
'                                    I[WorkTypeNumber + the number of detected workpieces -1]

' Flow of program
‘' Move to P[RobotImageAquisitionNumber]
'  -> Making snapshots & processing images.
'  -> Moving between the position of detected workpiece and P[PlaceNumber] repeatedly. 
'      (Repeat the motion up to the number of detected workpieces)
'  -> Move to P[RobotImageAquisitionNumber]

'------------Setting-------------

' EVP project file name
#Define EVPProjectFileName "project.evp"

' Height from the workpiece (safety margin for test run) [mm]
#Define WorkHeight 20

' Workpiece height from the reference surface (the bottom of COBOTTA)
' Example) The height of workpiece 1 is 20, workpiece 2 is 10.
' 	#Define ObjectHeight 20,10
' Example) The height of workpiece 1 is 20
#Define ObjectHeight 20

' P-type variable number of the robot position at the image acquisition.
#Define RobotImageAquisitionNumber 0

'P-type variable number of the place position.
#Define PlaceNumber 1

'I-type variable number of the number of detected workpieces.
#Define NumberOfWorks 0

'The start number among P-type variable numbers to store detected workpiece positions.
#Define WorkPlaceNumber 10

'The start number among I-type variable numbers to store the detected workpiece types.
#Define WorkTypeNumber 10

'-----------------------------

#Include "Variant.h"
Dim ctrl as Object

Sub Main
  Dim index As Long

  TakeArm Keep = 0

  ' Specify the height of workpiece (you must set the height here, not in the inside of EVP project.)
  Dim objHeightArray as Variant
  objHeightArray = Array(ObjectHeight)

  ' EVP initial setting (selecting an EVP project file, setting the height of workpieces)
  EVPInitialize2 EVPProjectFileName, objHeightArray
  
  ' Move to the robot position P[0] where snapshots have been taken.
  Move P, P[RobotImageAquisitionNumber] 
  
  ' Taking snapshots & image processing& acquiring workpieces
  EVPRun2
  
  ' Repeat this operation to the number of detected workpieces.
  For index = 0 To I[NumberOfWorks] - 1
    
    '------- Move to the detected workpiece position -------------------------
    ' Approach motion.  Move to 100mm above of P[10 + index].
    Approach P, P[WorkPlaceNumber + index], @0 100
    ' Descending motion.  Go down to P[10 + index].
    Move L, @C P[WorkPlaceNumber + index] 
    'Write gripping or releasing motion here, if necessary.
    Delay 500
    ' Ascending motion  100mm
    Depart L, @0 100
    
    '------- Robot motion at the place position  -------------------------
    'Approach motion.  Move to 100mm above of P[1] position.
    Approach P, P[PlaceNumber], @0 100
    ' Descending motion.  Go down to P[1].
    Move L, @C P[PlaceNumber] 
    'Write gripping or releasing motion here, if necessary.
    Delay 500
    ' Ascending motion  100mm
    Depart L, @0 100
    
  Next
  
  'Move to the robot position where snapshots have been taken.
   Move P, P[RobotImageAquisitionNumber] 
  
End Sub

Sub EVPInitialize2 ( EVPProjectFile as string, ByVal objHeightArray as Variant)
  ctrl = Cao.AddController("Runner", "CaoProv.DENSO.EVP", "", "project=" & EVPProjectFile)
    
  'Posture at the calibration
  Dim initPos as Position
  initPos = HandEyeInitPos()

  Dim n as Integer
  Dim objHeight as Integer

  'Set the height of each workpiece 
  For n = LBound(objHeightArray) To UBound(objHeightArray)
    objHeight = objHeightArray(n)
    ctrl.SetObjectHeight n, (objHeight + PosZ(initPos) - PosZ(P[RobotImageAquisitionNumber]))
  Next
    
End Sub

Sub EVPRun2

  Dim index As Long
  Dim ReturnValue As Variant
  Dim vntWorks As Variant
  Dim vntWork As Variant

  ' Taking snapshots & image processing
  ' =======================================
  ' Detailed information : Specifications of the return value of Run command
  ' Return value of Run Command.
  ' result <VT_VARIANT | VT_ARRAY> =
  '     Work Coun   ... <VT_I4>
  '     Work Datas  ... <Work Data | VT_ARRAY>
  '     Work Data   ... <VT_VARIANT | VT_ARRAY>
  '                          (classId, P(x, y, z, rx, ry, rz, fig))
  '     Feeder Action	... <VT_I4>
  ' =======================================
  ReturnValue = ctrl.Run()

  ' Store the number of detected workpieces into I[0].
  I[NumberOfWorks] = ReturnValue( 0 )

  ' Store the workpiece position into P[10], P[11], ..., P[10+the number of detected workpieces-1].
  vntWorks = ReturnValue( 1 )
  For index = 0 To I[NumberOfWorks] - 1
    vntWork = vntWorks(index)

    'Correct workpiece positions
    P[WorkPlaceNumber + index] = HandEyeConvPos( vntWork( 1 ) )

    'Store the height of workpieces to P[10], P[11], ..., P[10+the number of detected workpieces-1].
    LetZ P[WorkPlaceNumber + index] = PosZ(P[WorkPlaceNumber + index]) + WorkHeight
    Next
End Sub