VERSION 5.00
Begin VB.Form frm3DTracking 
   Caption         =   "3DTracking"
   ClientHeight    =   4185
   ClientLeft      =   60
   ClientTop       =   450
   ClientWidth     =   5445
   LinkTopic       =   "Form1"
   ScaleHeight     =   4185
   ScaleWidth      =   5445
   StartUpPosition =   3  'Windows の既定値
   Begin VB.Frame Frame2 
      Caption         =   "Camera"
      Height          =   2595
      Left            =   90
      TabIndex        =   5
      Top             =   1410
      Width           =   5265
      Begin VB.Timer timSampling 
         Enabled         =   0   'False
         Interval        =   500
         Left            =   600
         Top             =   150
      End
      Begin VB.CommandButton cmdCamConnect 
         Caption         =   "Connect"
         Height          =   375
         Left            =   2550
         TabIndex        =   20
         Top             =   210
         Width           =   1215
      End
      Begin VB.CommandButton cmdCamDisconnect 
         Caption         =   "Disconnect"
         Height          =   375
         Left            =   3870
         TabIndex        =   19
         Top             =   210
         Width           =   1215
      End
      Begin VB.TextBox txtDistance 
         Height          =   285
         Left            =   1110
         TabIndex        =   17
         Top             =   2100
         Width           =   975
      End
      Begin VB.TextBox txtRet 
         Height          =   285
         Index           =   2
         Left            =   4170
         TabIndex        =   16
         Top             =   1680
         Width           =   915
      End
      Begin VB.TextBox txtRet 
         Height          =   285
         Index           =   1
         Left            =   2520
         TabIndex        =   14
         Top             =   1680
         Width           =   975
      End
      Begin VB.TextBox txtRet 
         Height          =   285
         Index           =   0
         Left            =   1110
         TabIndex        =   12
         Top             =   1680
         Width           =   975
      End
      Begin VB.CheckBox chkRelative 
         Caption         =   "relative"
         Height          =   285
         Left            =   450
         TabIndex        =   10
         Top             =   1140
         Width           =   1785
      End
      Begin VB.CommandButton cmdStop 
         Caption         =   "Stop"
         Height          =   375
         Left            =   3870
         TabIndex        =   9
         Top             =   720
         Width           =   1215
      End
      Begin VB.CommandButton cmdStart 
         Caption         =   "Start"
         Height          =   375
         Left            =   2550
         TabIndex        =   8
         Top             =   720
         Width           =   1215
      End
      Begin VB.TextBox txtID 
         Alignment       =   1  '右揃え
         Height          =   315
         Left            =   1290
         TabIndex        =   7
         Text            =   "11"
         Top             =   750
         Width           =   1065
      End
      Begin VB.Label Label1 
         Caption         =   "Distance : "
         Height          =   165
         Index           =   3
         Left            =   270
         TabIndex        =   18
         Top             =   2160
         Width           =   795
      End
      Begin VB.Label Label1 
         Caption         =   "Angle : "
         Height          =   165
         Index           =   2
         Left            =   3600
         TabIndex        =   15
         Top             =   1740
         Width           =   585
      End
      Begin VB.Label Label1 
         Caption         =   "Y : "
         Height          =   165
         Index           =   1
         Left            =   2250
         TabIndex        =   13
         Top             =   1740
         Width           =   285
      End
      Begin VB.Label Label1 
         Caption         =   "X : "
         Height          =   165
         Index           =   0
         Left            =   810
         TabIndex        =   11
         Top             =   1710
         Width           =   285
      End
      Begin VB.Label Label3 
         Alignment       =   1  '右揃え
         Caption         =   "Image ID : "
         Height          =   255
         Left            =   240
         TabIndex        =   6
         Top             =   810
         Width           =   975
      End
   End
   Begin VB.Frame Frame1 
      Caption         =   "Robot"
      Height          =   1245
      Left            =   90
      TabIndex        =   0
      Top             =   90
      Width           =   5265
      Begin VB.ComboBox cmbParameter 
         Height          =   300
         Left            =   1320
         TabIndex        =   3
         Top             =   360
         Width           =   3765
      End
      Begin VB.CommandButton cmdRbtDisconnect 
         Caption         =   "Disconnect"
         Height          =   375
         Left            =   3840
         TabIndex        =   2
         Top             =   720
         Width           =   1215
      End
      Begin VB.CommandButton cmdRbtConnect 
         Caption         =   "Connect"
         Height          =   375
         Left            =   2550
         TabIndex        =   1
         Top             =   720
         Width           =   1215
      End
      Begin VB.Label Label2 
         Alignment       =   1  '右揃え
         Caption         =   "Parameter : "
         Height          =   255
         Left            =   240
         TabIndex        =   4
         Top             =   360
         Width           =   975
      End
   End
End
Attribute VB_Name = "frm3DTracking"
Attribute VB_GlobalNameSpace = False
Attribute VB_Creatable = False
Attribute VB_PredeclaredId = True
Attribute VB_Exposed = False
Option Explicit

Private caoEng As CaoEngine
Private caoCtrls As CaoControllers

Private caoCtrlNetwoRC As CaoController
Private caoRob As CaoRobot

Private caoCtrlOcv As CaoController
Private caoCmd As CaoCommand
Private caoFile As caoFile

Private CamsCenter(2) As Double

Private Sub Form_Load()
    
    With cmbParameter
        .Clear
        .AddItem "eth:192.168.0.1:4112" ' <- Change this for your controller IP setting. "eth:<controller IP>:4112
        .AddItem "com:1:38400:N:8:1" ' <- Change this for your controller RS-232C setting. "com:<COM port no.>:<speed>:N:8:1"
        .ListIndex = 0
    End With
    
    ' CAOオブジェクト生成
    Set caoEng = New CaoEngine
    Set caoCtrls = caoEng.Workspaces(0).Controllers
    
    cmdRbtDisconnect_Click
    cmdCamDisconnect_Click
   
End Sub

' ロボット接続
Private Sub cmdRbtConnect_Click()

    On Error GoTo ErrProc
    
    Dim i As Long
    
    cmdRbtDisconnect_Click
    
    ' NetwoRC接続
    Set caoCtrlNetwoRC = caoCtrls.Add("RC1", "CaoProv.DENSO.NetwoRC", "", "Conn=" & cmbParameter.Text)
    Set caoRob = caoCtrlNetwoRC.AddRobot("RobSlave")
    
    ' 初期位置に移動
    caoRob.Move 1, "J10"
     
    ' 表示設定
    cmdRbtConnect.Enabled = False
    cmdRbtDisconnect.Enabled = True
    
    Exit Sub
ErrProc:
    MsgBox Err.Description
    cmdRbtDisconnect_Click

End Sub

'ロボット切断
Private Sub cmdRbtDisconnect_Click()

    ' 表示設定
    cmdRbtConnect.Enabled = True
    cmdRbtDisconnect.Enabled = False

    ' NetwoRC切断
    If Not caoRob Is Nothing Then
        Set caoRob = Nothing
    End If

    If Not caoCtrlNetwoRC Is Nothing Then
        caoCtrls.Remove caoCtrlNetwoRC.Index
        Set caoCtrlNetwoRC = Nothing
    End If
    
End Sub

' カメラ接続
Private Sub cmdCamConnect_Click()

    On Error GoTo ErrProc
    
    Dim i As Long

    ' OpenCV接続
    Set caoCtrlOcv = caoCtrls.Add("Ocv", "CaoProv.OpenCV", "", "")
    Set caoCmd = caoCtrlOcv.AddCommand("TriMatchShapes")
    Set caoFile = caoCtrlOcv.AddFile("Image", "ID=1")
    
    ' 2つのカメラの中点を計算
    Dim CamPos1 As Variant
    Dim CamPos2 As Variant
    caoFile.ID = 1
    CamPos1 = caoFile.Execute("GetCamCalExtDat", False)
    caoFile.ID = 2
    CamPos2 = caoFile.Execute("GetCamCalExtDat", False)
    For i = 0 To 2
        CamsCenter(i) = (CamPos1(9 + i) + CamPos2(9 + i)) / 2
    Next
    caoFile.ID = 1
    
    cmdCamConnect.Enabled = False
    cmdCamDisconnect.Enabled = True
    cmdStart.Enabled = True
    cmdStop.Enabled = False
    
    Exit Sub
ErrProc:
    MsgBox Err.Description
    cmdRbtDisconnect_Click
    
End Sub

' カメラ切断
Private Sub cmdCamDisconnect_Click()

    cmdStop_Click

    ' 表示設定
    cmdCamConnect.Enabled = True
    cmdCamDisconnect.Enabled = False
    cmdStart.Enabled = False
    cmdStop.Enabled = False

    ' OpenCV切断
    If Not caoCmd Is Nothing Then
        Set caoCmd = Nothing
    End If

    If Not caoFile Is Nothing Then
        Set caoFile = Nothing
    End If
    
    If Not caoCtrlOcv Is Nothing Then
        caoCtrls.Remove caoCtrlOcv.Index
        Set caoCtrlOcv = Nothing
    End If

End Sub

'サンプリング開始
Private Sub cmdStart_Click()

    ' 表示設定
    cmdStart.Enabled = False
    cmdStop.Enabled = True
    
    ' パラメータ設定
'    caoCmd.Parameters = Array(1, 2, 0, txtID.Text, 125, 1, 2, 0.5, 0.001, False)
     caoCmd.Parameters = Array(1, 2, 0, txtID.Text, 235, 1, 2, 0.5, 0.2, False)
    
    ' サンプリング開始
    timSampling.Enabled = True
    
End Sub

'サンプリング停止
Private Sub cmdStop_Click()
    
    ' サンプリング停止
    timSampling.Enabled = False
    
    ' 表示設定
    cmdStart.Enabled = True
    cmdStop.Enabled = False
    
End Sub

'サンプリングタイマ
Private Sub timSampling_Timer()

    Dim vntPos As Variant
    
    On Error GoTo ErrProc
    
    ' TriMatchShapesで座標取得
    caoCmd.Execute 0
    vntPos = caoCmd.Result
    
    ' カメラ中点との距離計測
    txtDistance.Text = caoFile.Execute("Distance", Array(CamsCenter, vntPos))
        
    If Not caoRob Is Nothing Then
        ' ロボットを動かしてターゲットに接近
        If chkRelative.Value = 0 Then
            ' 絶対移動
            caoRob.Move 1, "P(" & vntPos(0) & "," & vntPos(1) & "," & vntPos(2) & ", 0, 0, 0, -1)"
    
        Else
            ' 相対移動
            caoRob.Draw 1, "V(" & -vntPos(0) & "," & -vntPos(1) & "," & -vntPos(2) & ")"
    
        End If
    End If

    ' MatchTemplateでロゴの位置を取得
'    vntPos = caoFile.Execute("MatchTemplate2", Array(txtID.Text + 1, 3, 0.8, 0, 360, 3))
    
    ' 結果表示
    txtRet(0).Text = vntPos(0)
    txtRet(1).Text = vntPos(1)
    txtRet(2).Text = vntPos(2)
    
    
    Exit Sub
ErrProc:
    ' エラー時は何もしないで終了
    MsgBox Err.Description
    cmdStop_Click

End Sub