VERSION 5.00
Object = "{5E9E78A0-531B-11CF-91F6-C2863C385E30}#1.0#0"; "MSFLXGRD.OCX"
Begin VB.Form frmSample 
   BorderStyle     =   3  '固定ﾀﾞｲｱﾛｸﾞ
   Caption         =   "Sample"
   ClientHeight    =   3120
   ClientLeft      =   45
   ClientTop       =   435
   ClientWidth     =   6225
   LinkTopic       =   "Form1"
   LockControls    =   -1  'True
   MaxButton       =   0   'False
   MinButton       =   0   'False
   ScaleHeight     =   3120
   ScaleWidth      =   6225
   ShowInTaskbar   =   0   'False
   StartUpPosition =   3  'Windows の既定値
   Begin VB.CheckBox chkRotate 
      Caption         =   "Use Rotate"
      Height          =   315
      Left            =   2100
      TabIndex        =   27
      Top             =   630
      Width           =   1305
   End
   Begin VB.TextBox txtIncrements 
      Alignment       =   1  '右揃え
      Height          =   270
      Left            =   5250
      TabIndex        =   26
      Text            =   "500"
      Top             =   2700
      Width           =   555
   End
   Begin VB.CommandButton cmdInitValue 
      Caption         =   "Init Base "
      Height          =   375
      Left            =   2880
      TabIndex        =   24
      Top             =   2670
      Width           =   1275
   End
   Begin VB.TextBox txtFilterValue 
      Alignment       =   1  '右揃え
      Height          =   270
      Left            =   900
      TabIndex        =   22
      Text            =   "50"
      Top             =   2700
      Width           =   555
   End
   Begin VB.CommandButton cmdCalcFilter 
      Caption         =   "Calc. Filter"
      Height          =   375
      Left            =   1530
      TabIndex        =   21
      Top             =   2670
      Width           =   1275
   End
   Begin VB.TextBox txtFilterLo 
      Height          =   270
      Index           =   5
      Left            =   5040
      TabIndex        =   20
      Top             =   2310
      Width           =   915
   End
   Begin VB.TextBox txtFilterLo 
      Height          =   270
      Index           =   4
      Left            =   4050
      TabIndex        =   19
      Top             =   2310
      Width           =   915
   End
   Begin VB.TextBox txtFilterLo 
      Height          =   270
      Index           =   3
      Left            =   3060
      TabIndex        =   18
      Top             =   2310
      Width           =   915
   End
   Begin VB.TextBox txtFilterLo 
      Height          =   270
      Index           =   2
      Left            =   2070
      TabIndex        =   17
      Top             =   2310
      Width           =   915
   End
   Begin VB.TextBox txtFilterLo 
      Height          =   270
      Index           =   1
      Left            =   1080
      TabIndex        =   16
      Top             =   2310
      Width           =   915
   End
   Begin VB.TextBox txtFilterLo 
      Height          =   270
      Index           =   0
      Left            =   90
      TabIndex        =   15
      Top             =   2310
      Width           =   915
   End
   Begin VB.TextBox txtFilterHi 
      Height          =   270
      Index           =   5
      Left            =   5040
      TabIndex        =   14
      Top             =   1020
      Width           =   915
   End
   Begin VB.TextBox txtFilterHi 
      Height          =   270
      Index           =   4
      Left            =   4050
      TabIndex        =   13
      Top             =   1020
      Width           =   915
   End
   Begin VB.TextBox txtFilterHi 
      Height          =   270
      Index           =   3
      Left            =   3060
      TabIndex        =   12
      Top             =   1020
      Width           =   915
   End
   Begin VB.TextBox txtFilterHi 
      Height          =   270
      Index           =   2
      Left            =   2070
      TabIndex        =   11
      Top             =   1020
      Width           =   915
   End
   Begin VB.TextBox txtFilterHi 
      Height          =   270
      Index           =   1
      Left            =   1080
      TabIndex        =   10
      Top             =   1020
      Width           =   915
   End
   Begin VB.TextBox txtFilterHi 
      Height          =   270
      Index           =   0
      Left            =   90
      TabIndex        =   9
      Top             =   1020
      Width           =   915
   End
   Begin VB.TextBox txtWDFID 
      Alignment       =   1  '右揃え
      Height          =   285
      IMEMode         =   3  'ｵﾌ固定
      Left            =   1350
      TabIndex        =   7
      Text            =   "1"
      Top             =   630
      Width           =   615
   End
   Begin VB.Timer Timer1 
      Left            =   5670
      Top             =   1830
   End
   Begin VB.ComboBox cmbParameter 
      Height          =   300
      Left            =   1350
      TabIndex        =   5
      Top             =   180
      Width           =   2085
   End
   Begin VB.CommandButton cmdDisconnect 
      Caption         =   "Disconnect"
      Height          =   375
      Left            =   4860
      TabIndex        =   4
      Top             =   150
      Width           =   1275
   End
   Begin VB.CommandButton cmdConnect 
      Caption         =   "Connect"
      Height          =   375
      Left            =   3540
      TabIndex        =   3
      Top             =   150
      Width           =   1275
   End
   Begin VB.CommandButton cmdStop 
      Caption         =   "Stop"
      Height          =   375
      Left            =   4860
      TabIndex        =   2
      Top             =   600
      Width           =   1275
   End
   Begin VB.CommandButton cmdStart 
      Caption         =   "Start"
      Height          =   375
      Left            =   3540
      TabIndex        =   1
      Top             =   600
      Width           =   1275
   End
   Begin MSFlexGridLib.MSFlexGrid grdPower 
      Height          =   975
      Left            =   90
      TabIndex        =   0
      Top             =   1320
      Width           =   6045
      _ExtentX        =   10663
      _ExtentY        =   1720
      _Version        =   393216
      Appearance      =   0
   End
   Begin VB.Label Label4 
      Alignment       =   1  '右揃え
      Caption         =   "Increments : "
      Height          =   255
      Left            =   4230
      TabIndex        =   25
      Top             =   2730
      Width           =   1005
   End
   Begin VB.Label Label3 
      Alignment       =   1  '右揃え
      Caption         =   "Filter : "
      Height          =   255
      Left            =   120
      TabIndex        =   23
      Top             =   2730
      Width           =   765
   End
   Begin VB.Label Label1 
      Alignment       =   1  '右揃え
      Caption         =   "WDF-6A  ID : "
      Height          =   255
      Left            =   90
      TabIndex        =   8
      Top             =   690
      Width           =   1245
   End
   Begin VB.Label Label2 
      Alignment       =   1  '右揃え
      Caption         =   "Controller : "
      Height          =   255
      Left            =   90
      TabIndex        =   6
      Top             =   240
      Width           =   1245
   End
End
Attribute VB_Name = "frmSample"
Attribute VB_GlobalNameSpace = False
Attribute VB_Creatable = False
Attribute VB_PredeclaredId = True
Attribute VB_Exposed = False
Option Explicit

' 力覚センサの値の平均化する要素数
Private Const HISTORY_VALUE = 19

' CaoEngine
Private m_caoEng As New CaoEngine

' Robot
Private m_caoCtrls As CaoControllers
Private m_caoCtrl  As CaoController
Attribute m_caoCtrl.VB_VarHelpID = -1
Private m_caoTask As CaoTask
Private m_caoRob  As CaoRobot
Private m_caoRobVar As CaoVariable
Private m_CurPose As CaoVariable

' WDF-6A
Private m_caoWDFCtrl As CaoController
Private m_caoWDFData As CaoVariable
Private m_caoWDFLastData As CaoVariable
Private m_caoWDFSngData As CaoVariable

' 現在値
Private m_vMyCurPos(0 To 6) As Single

' 力覚センサの基準値
Private m_vntBase As Variant

' Start,Stopのフラグ
Private m_bState As Boolean


Private Sub Form_Load()

    Dim i As Long

    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

    With grdPower
        .Rows = 2
        .Cols = 6
        
        .FixedCols = 0
        .FixedRows = 1
        
        For i = 0 To .Cols - 1
            .ColWidth(i) = (.Width / 6) - 30
        Next
        For i = 0 To .Rows - 1
            .RowHeight(i) = (.Height / 2) - 30
        Next
        
        .TextMatrix(0, 0) = "X"
        .TextMatrix(0, 1) = "Y"
        .TextMatrix(0, 2) = "Z"
        .TextMatrix(0, 3) = "RX"
        .TextMatrix(0, 4) = "RY"
        .TextMatrix(0, 5) = "RZ"
        
    End With
    
    With Timer1
        .Enabled = False
        .Interval = 10
    End With
    
    cmdDisconnect_Click

    Set m_caoEng = New CaoEngine
    Set m_caoCtrls = m_caoEng.Workspaces(0).Controllers

    cmdStop.Enabled = False
    cmdCalcFilter.Enabled = False
    cmdInitValue.Enabled = False

End Sub

' ----------------------------------------------------
Private Sub Form_Unload(Cancel As Integer)

    cmdStop_Click
    cmdDisconnect_Click
    
    Set m_caoCtrls = Nothing
    Set m_caoEng = Nothing
    
End Sub

' ----------------------------------------------------
Private Sub cmdConnect_Click()

    On Error GoTo ErrProc
    
    Dim sConn As String
    
    cmdDisconnect_Click
    
    sConn = "Conn=" & cmbParameter.Text
    
    Set m_caoCtrl = m_caoCtrls.Add("RC1", "CaoProv.DENSO.NetwoRC", "", sConn)
    Set m_caoRob = m_caoCtrl.AddRobot("RobSlave")
    Set m_caoRobVar = m_caoRob.AddVariable("@BUSY_STATUS")
    Set m_CurPose = m_caoRob.AddVariable("@CURRENT_POSITION")
        
    cmdConnect.Enabled = False
    cmdDisconnect.Enabled = True
    
    ' 現在値を取得
    InitCurPos
        
    Exit Sub
ErrProc:
    MsgBox Err.Description
    
End Sub

' ----------------------------------------------------
Private Sub cmdDisconnect_Click()

    Timer1.Enabled = False

    If Not m_caoWDFSngData Is Nothing Then
        Set m_caoWDFSngData = Nothing
    End If

    If Not m_caoWDFLastData Is Nothing Then
        Set m_caoWDFLastData = Nothing
    End If
    
    If Not m_caoWDFData Is Nothing Then
        Set m_caoWDFData = Nothing
    End If
    
    If Not m_caoWDFCtrl Is Nothing Then
        m_caoCtrls.Remove m_caoWDFCtrl.Index
        Set m_caoWDFCtrl = Nothing
    End If

    If Not m_CurPose Is Nothing Then
        Set m_CurPose = Nothing
    End If

    If Not m_caoRobVar Is Nothing Then
        Set m_caoRobVar = Nothing
    End If

    If Not m_caoRob Is Nothing Then
        Set m_caoRob = Nothing
    End If

    If Not m_caoCtrl Is Nothing Then
        m_caoCtrls.Remove m_caoCtrl.Index
        Set m_caoCtrl = Nothing
    End If
    
    cmdConnect.Enabled = True
    cmdDisconnect.Enabled = False
    
End Sub


' 不感帯の計算 (1000回のデータ平均を基準値とし，±FilterValueを不感帯とする)
Private Sub cmdCalcFilter_Click()

    ' 1000回の値をFilterテキストにセット
    SetFilterValue GetCustomCountAvg(999)

End Sub


' Filterテキストに値を表示
Private Sub SetFilterValue(ByVal vntValue As Variant)

    Dim i As Long

    ' 不感帯テキストに表示
    For i = 0 To 5
        txtFilterHi(i).Text = vntValue(i + 1) + txtFilterValue.Text
        txtFilterLo(i).Text = vntValue(i + 1) - txtFilterValue.Text
    Next

End Sub


' 力覚センサの基準値の取得
Private Sub cmdInitValue_Click()
    
    ' 1000回の平均を基準値とする
    m_vntBase = GetCustomCountAvg(999)

End Sub


' 開始を押下
Private Sub cmdStart_Click()

    On Error GoTo ErrProc

    ' Check if "ROBSLAVE" task is running
    If Not IsRobSlaveRunning() Then
        MsgBox """ROBSLAVE"" task is not running into the controller !"
        GoTo ErrProc
    End If

    ' WDF-6A 生成
    Set m_caoWDFCtrl = m_caoCtrls.Add("WDF-6A", "CaoProv.WACOH.WDF-6A", "", "Port=" & txtWDFID.Text & ",@IfNotMember=True")
    Set m_caoWDFData = m_caoWDFCtrl.AddVariable("@Data", "@IfNotMember=True")
    Set m_caoWDFLastData = m_caoWDFCtrl.AddVariable("@LastData", "@IfNotMember=True")
    Set m_caoWDFSngData = m_caoWDFCtrl.AddVariable("@SingleData", "@IfNotMember=True")

    Dim vntDat As Variant
    Dim i As Long
    
    ' フィルタのセット
    SetFilterValue vntDat

    m_bState = True
    cmdStart.Enabled = Not m_bState
    cmdStop.Enabled = m_bState
    cmdCalcFilter.Enabled = m_bState
    cmdInitValue.Enabled = m_bState

    ' Init Baseする
    cmdInitValue_Click

    ' センサの値取得開始
    MainLoop

    Exit Sub
ErrProc:
    cmdStop_Click
    
End Sub

' 停止を押下
Private Sub cmdStop_Click()

    m_bState = False
    cmdStart.Enabled = Not m_bState
    cmdStop.Enabled = m_bState
    cmdCalcFilter.Enabled = m_bState
    cmdInitValue.Enabled = m_bState

    Set m_caoWDFSngData = Nothing
    Set m_caoWDFLastData = Nothing
    Set m_caoWDFData = Nothing
    Set m_caoWDFCtrl = Nothing

End Sub


' Check if "ROBSLAVE" task is running
Private Function IsRobSlaveRunning() As Boolean

    On Error GoTo ErrExit

    Dim Task As CaoTask
    Dim Var As CaoVariable
    
    IsRobSlaveRunning = False
    
    Set Task = m_caoCtrl.AddTask("ROBSLAVE")
    Set Var = Task.AddVariable("@STATUS")
    
    If Var.Value = 3 Then 'Running
        IsRobSlaveRunning = True
    End If
    
    Task.Variables.Remove Var.Index
    m_caoCtrl.Tasks.Remove Task.Index
    
    Exit Function
ErrExit:
    
End Function


' 現在値を取得
Private Sub InitCurPos()

    ' CurrentPositionの位置の取得　P型なので要素数は決めうち
    m_vMyCurPos(0) = CSng(m_CurPose(0))
    m_vMyCurPos(1) = CSng(m_CurPose(1))
    m_vMyCurPos(2) = CSng(m_CurPose(2))
    m_vMyCurPos(3) = CSng(m_CurPose(3))
    m_vMyCurPos(4) = CSng(m_CurPose(4))
    m_vMyCurPos(5) = CSng(m_CurPose(5))
    m_vMyCurPos(6) = -1 'CSng(m_CurPose(6))

End Sub


' タイマーでは間に合わないので，無限ループ
Private Sub MainLoop()

    Dim vntDat As Variant
    Dim i As Long

    Do
        DoEvents

        ' ループ終了フラグのチェック
        If m_bState = False Then
            Exit Sub
        End If

        ' 値の取得
        vntDat = vbEmpty
        vntDat = m_caoWDFSngData.Value
        If Not IsEmpty(vntDat) Then         ' 取得データなしの時はS_FALSEでEmptyを返す．
            
            ' データの抜けチェック
            If (vntDat(0) > 0) Then
                
                ' データ平均化
                vntDat = GetAvg(vntDat)
                                            
                ' ロボットの移動
                MoveRobot vntDat
                    
                ' 値の表示
                For i = 0 To 5
                    grdPower.TextMatrix(1, i) = vntDat(i + 1)
                Next
            
            End If
        
        End If
    Loop

End Sub


' ロボット動作
Private Sub MoveRobot(ByVal vntValue As Variant)

    Dim i As Long
    Dim lInc As Long
    Dim vCurPose As Variant
    Dim sngDest(0 To 5) As Single

    For i = 0 To 5
        
        ' 不感帯のチェック
        If (CheckFilter(vntValue(i + 1), i)) Then
            
            ' 動作刻みのチェック
            If (IsNumeric(txtIncrements.Text)) Then
                lInc = CLng(txtIncrements.Text)
                If (50 <= lInc) Then
                    sngDest(i) = (vntValue(i + 1) - m_vntBase(i + 1)) / lInc
                End If
            End If
        
        End If

    Next

    ' ロボットの現在値の取得
    vCurPose = m_vMyCurPos

    ' 力学センサで取得してきた値から距離を考慮して現在値に足しこむ　　(現在はXYZのみ)
    vCurPose(0) = vCurPose(0) + sngDest(0)
    vCurPose(1) = vCurPose(1) - sngDest(1)
    vCurPose(2) = vCurPose(2) - sngDest(2)
    vCurPose(3) = vCurPose(3) + IIf((chkRotate.Value = vbChecked), sngDest(3), 0)
    vCurPose(4) = vCurPose(4) + IIf((chkRotate.Value = vbChecked), sngDest(4), 0)
    vCurPose(5) = vCurPose(5) + IIf((chkRotate.Value = vbChecked), sngDest(5), 0)
    vCurPose(6) = -1                            ' Figは常に不定 -1

    ' 実行
    On Error Resume Next
    m_caoRob.Move 1, Array(vCurPose, "P", "@P")

    ' 現在値更新
    m_vMyCurPos(0) = CSng(vCurPose(0))
    m_vMyCurPos(1) = CSng(vCurPose(1))
    m_vMyCurPos(2) = CSng(vCurPose(2))
    m_vMyCurPos(3) = CSng(vCurPose(3))
    m_vMyCurPos(4) = CSng(vCurPose(4))
    m_vMyCurPos(5) = CSng(vCurPose(5))
    m_vMyCurPos(6) = -1 ' CSng(vCurPose(6))


End Sub


' 指定回数の平均値を取得
Private Function GetCustomCountAvg(lCount As Long) As Variant

    Dim vntDat As Variant
    Dim i As Long

    For i = 0 To lCount
        
        ' 値の取得
        vntDat = m_caoWDFSngData.Value
        If Not IsEmpty(vntDat) Then ' 取得データなしの時はS_FALSEでEmptyを返す．
            
            ' データの抜けチェック
            If (vntDat(0) > 0) Then
                
                ' データ平均化
                vntDat = GetAvg(vntDat)
            
            End If
        
        End If
    Next

    GetCustomCountAvg = vntDat

End Function


' 力覚センサの値の不感帯チェックする  True⇒不感帯でない　False⇒不感帯
Private Function CheckFilter(ByVal lngValue As Long, lAxis As Long) As Boolean
    
    On Error Resume Next

    Dim i As Long

    CheckFilter = True

    ' 値が不感帯の間かどうか
    If (CLng(txtFilterLo(lAxis).Text) < lngValue) And (lngValue < CLng(txtFilterHi(lAxis).Text)) Then
        GoTo ValueIsFilter
    End If

    Exit Function
ValueIsFilter:
    CheckFilter = False

End Function


' 力覚センサの値をバッファに格納
Private Function GetAvg(ByVal vntValue As Variant) As Variant

    Dim vntData(0 To 6) As Variant
    Dim lSumData As Long, lDevValue As Long
    Dim i As Long, j As Long
    
    Static lCount As Long
    Static vntHisData(0 To HISTORY_VALUE) As Variant
    
    vntHisData(lCount) = vntValue
    
    ' 平均を求める
    For i = 1 To 6
        lDevValue = 0
        lSumData = 0
        For j = 0 To HISTORY_VALUE
            If (IsEmpty(vntHisData(j)) = False) Then
                lSumData = lSumData + vntHisData(j)(i)      ' Empty ⇒ まだ値が格納されていない
                lDevValue = lDevValue + 1
            End If
        Next
        
        vntData(i) = CLng(lSumData / lDevValue)
    Next
    
    ' カウンタを+1
    If (lCount < HISTORY_VALUE) Then
        lCount = lCount + 1
    Else
        lCount = 0
    End If

    ' 平均値を返す
    GetAvg = vntData

End Function