HandEyeConvPos

用例

'!TITLE "HandEyeEVP"

' アプローチ長	100mm
' デパート長	100mm

' ロボット待機位置	P[RobotImageAquisitionNumber]
'                       (EVPプロジェクト作成時のロボット姿勢を保存してください。
'                       また、ワークに対してカメラが垂直であることを確認してください。)
' プレース位置          P[PlaceNumber]

' EVPRun2にて、カメラ撮影＆画像処理＆ワーク位置取得し下記のデータに値をセット
' 見つかったワーク個数	I[NumberOfWorks]
' 見つかったワーク位置	P[WorkPlaceNumber], P[WorkPlaceNumber+1], ... ,
'                       P[WorkPlaceNumber+見つかったワーク個数-1]
' 見つかったワーク種類	I[WorkTypeNumber], I[WorkTypeNumber+1], ... , 
'                       I[WorkTypeNumber+見つかったワーク個数-1]

' プログラムの流れ
' P[RobotImageAquisitionNumber]へ移動
'  -> カメラ撮影＆画像処理
'  -> 見つかったワーク位置とP[PlaceNumber]を往復する。（見つかったワーク個数分だけ繰り返す）
'  -> P[RobotImageAquisitionNumber]へ移動

'------------設定-------------

' EVPプロジェクトファイル名
#Define EVPProjectFileName "project.evp"

' ワークからの高さ(テスト用安全マージン) [mm]
#Define WorkHeight 20

' 基準平面（COBOTTAの底面）からのワークの高さ
' 例)ワーク1の高さが20、ワーク2の高さが10のとき
'   #Define ObjectHeight 20,10
' 例)ワーク1の高さが20のとき
#Define ObjectHeight 20

' 画像取得時のロボット位置のP変数番号
#Define RobotImageAquisitionNumber 0

' プレース位置のP変数番号
#Define PlaceNumber 1

' 見つかったワーク個数のI変数番号
#Define NumberOfWorks 0

' 見つかったワーク位置のP変数開始番号
#Define WorkPlaceNumber 10

' 見つかったワーク種類のI変数開始番号
#Define WorkTypeNumber 10

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

#Include "Variant.h"
Dim ctrl as Object

Sub Main
  Dim index As Long

  TakeArm Keep = 0

  ' ワークの高さを指定（EVPプロジェクト内ではなくここで高さを設定する）
  Dim objHeightArray as Variant
  objHeightArray = Array(ObjectHeight)

  ' EVP初期設定 (EVPプロジェクトファイル選択、ワーク高さの設定)
  EVPInitialize2 EVPProjectFileName, objHeightArray
	
  ' 画像取得時のロボット位置P[0]へ移動
  Move P, P[RobotImageAquisitionNumber] 
	
  ' カメラ撮影＆画像処理＆ワーク位置取得
  EVPRun2
	
  ' 見つかったワーク個数分繰り返す
  For index = 0 To I[NumberOfWorks] - 1

    '------- 見つかったワーク位置へ移動 -------------------------
    'アプローチ動作  P[10 + index]上空100mmにアプローチ
    Approach P, P[WorkPlaceNumber + index], @0 100
    ' 下降動作    P[10 + index]に下降
    Move L, @C P[WorkPlaceNumber + index] 
    ' チャックやアンチャック動作等を必要に応じてここに記述
    Delay 500
    ' 上昇動作    100mm
    Depart L, @0 100
    
    '------- プレース位置での動作 -------------------------
    'アプローチ動作  P[1]上空100mmにアプローチ
    Approach P, P[PlaceNumber], @0 100
    ' 下降動作    P[1]に下降
    Move L, @C P[PlaceNumber] 
    ' チャックやアンチャック動作等を必要に応じてここに記述
    Delay 500
    ' 上昇動作    100mm
    Depart L, @0 100
    
  Next
  
  ' 画像取得時のロボット位置へ移動
  Move P, P[RobotImageAquisitionNumber] 
  
End Sub

Sub EVPInitialize2 ( EVPProjectFile as string, ByVal objHeightArray as Variant)
  ctrl = Cao.AddController("Runner", "CaoProv.DENSO.EVP", "", "project=" & _
  EVPProjectFile)
    
  ' キャリブレーション時の姿勢
  Dim initPos as Position
  initPos = HandEyeInitPos()

  Dim n as Integer
  Dim objHeight as Integer

  ' それぞれのワークの高さをセット
  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

  ' カメラ撮影＆画像処理
  ' =======================================
  ' 詳細情報：Runコマンドの返り値仕様
  ' Return value of Run Command.
  ' result <VT_VARIANT | VT_ARRAY> =
  '    Work Count	 	... <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()

  ' 見つかったワーク個数をI[0]へ格納
  I[NumberOfWorks] = ReturnValue( 0 )

  ' ワーク位置をP[10], P[11], ..., P[10+見つかったワーク個数-1]へ格納
  vntWorks = ReturnValue( 1 )
  For index = 0 To I[NumberOfWorks] - 1
    vntWork = vntWorks(index)

    ' ワーク位置を補正
    P[WorkPlaceNumber + index] = HandEyeConvPos( vntWork( 1 ) )

    ' ワークの高さをP[10], P[11], ..., P[10+見つかったワーク個数-1]へ設定
    LetZ P[WorkPlaceNumber + index] = PosZ(P[WorkPlaceNumber + index]) + _
         WorkHeight
  Next
End Sub