Control your Human-Like Robot from Anywhere you Are!

Imports System.Runtime.InteropServices
Imports System
Imports System.Threading
Imports System.IO.Ports
Imports System.ComponentModel
Imports System.Math
Imports System.Timers
Imports System.Net
Imports System.Net.Sockets
Imports System.Text

Public Class Form2
    Dim analogx As Integer
    Dim analogy As Integer
    Dim persenkecepatan As Integer
    Dim tombol As Integer
    Declare Function joyGetPosEx Lib "winmm.dll" (ByVal uJoyID As Integer, ByRef pji As JOYINFOEX) As Integer

    <StructLayout(LayoutKind.Sequential)> _
    Public Structure JOYINFOEX
        Public dwSize As Integer
        Public dwFlags As Integer
        Public dwXpos As Integer
        Public dwYpos As Integer
        Public dwZpos As Integer
        Public dwRpos As Integer
        Public dwUpos As Integer
        Public dwVpos As Integer
        Public dwButtons As Integer
        Public dwButtonNumber As Integer
        Public dwPOV As Integer
        Public dwReserved1 As Integer
        Public dwReserved2 As Integer
    End Structure

    Dim myjoyEX As JOYINFOEX

    Private Sub Form2_Load(sender As System.Object, e As System.EventArgs) Handles MyBase.Load
      

        myjoyEX.dwSize = 64
        myjoyEX.dwFlags = &HFF ' All information
        'Timer1.Interval = 200  'Update at 5 hz

    
    End Sub

    Private Sub initializeSender()
        sendingClient = New UdpClient(broadCastAddress, port)
        sendingClient.EnableBroadcast = True
    End Sub

    Private Sub initializeReceiver()

        receivingClient = New UdpClient(port)

        If (String.IsNullOrEmpty(port)) Then
            'messagebox.Show("port is not available")
        End If

        Dim start As ThreadStart = New ThreadStart(AddressOf receiver)
        receivingThread = New Thread(start)
        receivingThread.IsBackground = True
        receivingThread.Start()
    End Sub

    Private Sub receiver()
        Dim endpoint As IPEndPoint = New IPEndPoint(IPAddress.Any, port)
        Dim messageDelegate As addMessage = AddressOf MessageReceived

        While (True)
            Dim Data() As Byte
            Data = receivingClient.Receive(endpoint)
            Dim Message As String = Encoding.ASCII.GetString(Data)
            Invoke(messageDelegate, Message)

        End While

    End Sub

    Private Sub MessageReceived(ByRef message As String)
        'rtbChart.Text += message + vbNewLine
    End Sub


    Private Sub Timer1_Tick(sender As System.Object, e As System.EventArgs) Handles Timer1.Tick

    End Sub

    Private Sub Button1_Click(sender As System.Object, e As System.EventArgs) Handles Button1.Click
        'SerialPort1.Open()
        'Button1.Enabled = False
        Dim datasiapkirim() As Byte = Encoding.ASCII.GetBytes("testing")
        sendingClient.Send(datasiapkirim, datasiapkirim.Length)
    End Sub

    Private Sub Button2_Click(sender As System.Object, e As System.EventArgs) Handles Button2.Click
        Timer2.Enabled = True
        Button2.Enabled = False

    End Sub

    Private Sub Timer2_Tick(sender As System.Object, e As System.EventArgs) Handles Timer2.Tick
        ' Get the joystick information
        Call joyGetPosEx(0, myjoyEX)

        ListBox1.Items.Clear()
        With myjoyEX
            analogx = 255 / 65535 * .dwXpos
            analogy = 255 - (255 / 65535 * .dwYpos)
            If analogx >= 123 And analogx <= 133 Then analogx = 128
            If analogy >= 123 And analogy <= 133 Then analogy = 128

            persenkecepatan = (65535 - .dwZpos) / 65535 * 128
            tombol = .dwButtons

            ListBox1.Items.Add(.dwXpos.ToString & "-" & analogx)          'Up to six axis supported
            ListBox1.Items.Add(.dwYpos.ToString & "-" & analogy)
            ListBox1.Items.Add(.dwZpos.ToString & "-" & persenkecepatan)
            ListBox1.Items.Add(.dwButtons.ToString) '("X"))  'Print in Hex, so can see the individual bits associated with the buttons
            'ListBox1.Items.Add(.dwButtonNumber.ToString)  'number of buttons pressed at the same time
            'ListBox1.Items.Add((.dwPOV / 100).ToString)     'POV hat (in 1/100ths of degrees, so divided by 100 to give degrees)
        End With

        Dim itombol As Integer = 255
        Dim genggam As Integer = 0
        Dim cwccw As Integer = 0

        If tombol = 1 Then
            genggam = 1
        ElseIf tombol = 8 Then
            cwccw = 1
        ElseIf tombol = 16 Then
            cwccw = 2
        ElseIf tombol = 9 Then
            cwccw = 1
            genggam = 1
        ElseIf tombol = 17 Then
            cwccw = 2
            genggam = 1
        End If

       
        TextBox1.Text = "O" & analogy & "P" & analogx & "Q" & cwccw & "R" & genggam & "S"
        
        Dim datasiapkirim() As Byte = Encoding.ASCII.GetBytes(TextBox1.Text)
        sendingClient.Send(datasiapkirim, datasiapkirim.Length)

    End Sub
End Class

Imports System
Imports Microsoft.Kinect
Imports System.Threading
Imports System.IO.Ports
Imports System.ComponentModel
Imports System.Math
Imports System.Drawing
Imports System.Windows.Forms
Imports System.Timers
Imports System.Net
Imports System.Net.Sockets
Imports System.Text

Public Class Form1
    'Inherits Form
    Delegate Sub addMessage(ByRef message As String)
    Private username As String = "BuTO_Server"
    Const port As Integer = 2390
    Dim broadCastAddress As String

    Private receivingClient As UdpClient
    Private sendingClient As UdpClient
    Private receivingThread As Thread

    'konstanta servo
    '-------------------------------------------------------------------------------------------
    Public TAmbilData_Delegate, TSudutA_Delegate, TSudutB_Delegate, TSudutD_Delegate As Threading.TimerCallback
    Public TAmbilData, TSudutA, TSudutB, TSudutD As System.Threading.Timer

    Dim nilai, nilai2, nilai3 As Integer
    Dim keep1, keep2, keep3 As Integer
    Public a1, a2, a3, a4 As Single
    Dim konter_serial As Integer = 0
    Dim lastgrip_open, lastgrip_close As Integer


    'Dim WithEvents aTimer As New System.Threading.Timer(AddressOf ticktock, Nothing, 0, 500)
    'Dim WithEvents bTimer As New System.Threading.Timer(AddressOf ticktock2, Nothing, 0, 1000)

    Dim bahutengah_x, bahutengah_y, bahutengah_z As Integer
    Dim bahutengah_dx, bahutengah_dy, bahutengah_dz As Single
    Dim bahukanan_x, bahukanan_y, bahukanan_z As Integer
    Dim bahukanan_dx, bahukanan_dy, bahukanan_dz As Single
    Dim sikukanan_x, sikukanan_y, sikukanan_z As Integer
    Dim sikukanan_dx, sikukanan_dy, sikukanan_dz As Single
    Dim pergelangankanan_x, pergelangankanan_y, pergelangankanan_z As Integer
    Dim tangankanan_x, tangankanan_y, tangankanan_z As Integer

    Dim pergelangankanan_dx, pergelangankanan_dy, pergelangankanan_dz As Single

    Dim nservo1 As Integer = 2048
    Dim nservo2 As Integer = 2048
    Dim nservo3 As Integer = 512
    Dim nservo4 As Integer = 512

    Dim skalaplus, skalaplusb, skalaplusd As Integer

    Dim suduta1 As Integer = 0
    Dim suduta2 As Integer = 0
    Dim suduta3 As Integer = 0

    Dim lastservo1 As Integer = 2048
    Dim lastservo2 As Integer = 2048
    Dim lastservo3 As Integer = 512
    Dim lastservo4 As Integer = 512

    Dim lservo1, lservo2, lservo3, lservo4 As Integer

    'Complemntary Filter
    Dim klowpass, klowpass2 As String
    Dim khighpass, khighpass2 As String
    Dim dout1, dout2, dout3, dout4 As Integer

    'Konstanta Kinect
    '-----------------------------------------------------------------------------------
    Public TKinect_Delegate As Threading.TimerCallback
    Public TKinect As System.Threading.Timer

    'untuk kinect
    Dim kinz As KinectSensor
    Dim imagez As ColorImageFrame
    Dim skeletonz As SkeletonFrame
    Dim piccolor As Bitmap = New Bitmap(640, 480, Imaging.PixelFormat.Format32bppRgb)
    Dim depthz As DepthImageFrame
    Dim picdepth As Bitmap = New Bitmap(640, 480, Imaging.PixelFormat.Format32bppRgb)
    Dim picdepth_procc As Bitmap = New Bitmap(100, 100, Imaging.PixelFormat.Format32bppRgb)
    Dim picdepth_procc2 As Bitmap = New Bitmap(100, 100, Imaging.PixelFormat.Format32bppRgb)
    Dim gfx As Graphics = Graphics.FromImage(piccolor)
    Dim gfxd As Graphics = Graphics.FromImage(picdepth)
    Dim pixmax As Integer = 35000
    Dim last_servo As Integer

    'tampilan grafik
    Dim bmp As Bitmap
    Dim big_G As Graphics
    Dim pen As New Pen(Color.Red)

    'operasi sudut
    Dim konter As Integer = 0
    Dim x1, y1, x2, y2, x3, y3, x4, y4 As Integer
    'Dim a1, a2, a3 As Single
    Dim smooth As TransformSmoothParameters = New TransformSmoothParameters
    
    Private Sub Form1_Load(sender As System.Object, e As System.EventArgs) Handles MyBase.Load
        '----------------------------------------------------------
        Dim potentialsensor As Microsoft.Kinect.KinectSensor
        broadCastAddress = TxtAddres.Text

        smooth.Correction = 0.7F
        smooth.JitterRadius = 0.5F
        smooth.MaxDeviationRadius = 0.1F
        smooth.Prediction = 0.1F
        smooth.Smoothing = 0.5F

        For Each potentialsensor In KinectSensor.KinectSensors
            If potentialsensor.Status = KinectStatus.Connected Then
                Me.kinz = potentialsensor
                Exit For
            End If
        Next potentialsensor

        'Me.kinz.ColorStream.Enable(ColorImageFormat.RgbResolution640x480Fps30)

        'If KinectStatus.Connected = True Then

        'End If

        'Timerdelay.Enabled = True
        klowpass = ScrBar_KLowPass.Value / 100
        khighpass = 1 - klowpass

        ScrBar_KHighPass.Value = khighpass * 100

        klowpass2 = klowpass / 2.5
        khighpass2 = 1 - klowpass2

        TxtKLowPass.Text = klowpass
        TxtKHighPass.Text = khighpass

        initializeSender()
        initializeReceiver()

        'Form2.Show()

    End Sub

    Private Sub initializeSender()
        sendingClient = New UdpClient(broadCastAddress, port)
        sendingClient.EnableBroadcast = True
    End Sub

    Private Sub initializeReceiver()

        receivingClient = New UdpClient(port)

        If (String.IsNullOrEmpty(port)) Then
            MessageBox.Show("port is not available")
        End If

        Dim start As ThreadStart = New ThreadStart(AddressOf receiver)
        receivingThread = New Thread(start)
        receivingThread.IsBackground = True
        receivingThread.Start()
    End Sub

    Private Sub receiver()
        Dim endpoint As IPEndPoint = New IPEndPoint(IPAddress.Any, port)
        Dim messageDelegate As addMessage = AddressOf MessageReceived

        While (True)
            Dim Data() As Byte
            Data = receivingClient.Receive(endpoint)
            Dim Message As String = Encoding.ASCII.GetString(Data)
            Invoke(messageDelegate, Message)

        End While

    End Sub


    Private Sub MessageReceived(ByRef message As String)
        'rtbChart.Text += message + vbNewLine
    End Sub


    Private Sub Form1_Leave(sender As System.Object, e As System.EventArgs) Handles MyBase.Leave
        Me.kinz.Stop()
    End Sub

    'Complementary Filter
    Private Sub ScrBar_KLowPass_Scroll(sender As System.Object, e As System.Windows.Forms.ScrollEventArgs) Handles ScrBar_KLowPass.Scroll
        klowpass = ScrBar_KLowPass.Value / 100
        khighpass = 1 - klowpass

        ScrBar_KHighPass.Value = khighpass * 100

        klowpass2 = klowpass / 2.5
        khighpass2 = 1 - klowpass2

        TxtKLowPass.Text = klowpass
        TxtKHighPass.Text = khighpass

    End Sub

    'Sub servo 
    '~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
    Private Sub Button1_Click(sender As System.Object, e As System.EventArgs) Handles BtnBukaPort.Click
        If dxl_initialize(3, 1) = 1 Then
            'Set goal speed	
            dxl_write_word(1, P_GOAL_SPEED_L, 180) '270
            dxl_write_word(2, P_GOAL_SPEED_L, 180) '270
            dxl_write_word(3, P_GOAL_SPEED_L, 270) '270
            dxl_write_word(4, P_GOAL_SPEED_L, 270) '270


            'Set goal position
            dxl_write_word(1, P_GOAL_POSITION_L, 2100)
            dxl_write_word(2, P_GOAL_POSITION_L, 2150)
            dxl_write_word(3, P_GOAL_POSITION_L, 512)
            dxl_write_word(4, P_GOAL_POSITION_L, 512)
            dxl_write_word(5, P_GOAL_POSITION_L, 512)
        Else
            MsgBox("cek koneksi")
        End If

    End Sub

    Private Sub BtnDataSent_Click(sender As System.Object, e As System.EventArgs) Handles BtnDataSent.Click
        'TSudutA = New System.Threading.Timer(TSudutA_Delegate, Nothing, 0, 100)
        'TSudutB = New System.Threading.Timer(TSudutB_Delegate, Nothing, 0, 100)
        'TSudutD = New System.Threading.Timer(TSudutD_Delegate, Nothing, 0, 100)
        TimerKirimData.Enabled = True
        BtnDataSent.Enabled = False

    End Sub

    Private Sub VScrollBar6_Scroll(sender As System.Object, e As System.Windows.Forms.ScrollEventArgs) Handles VScrollBar6.Scroll
        TextBox6.Text = VScrollBar6.Value

    End Sub

    Private Sub VScrollBar5_Scroll(sender As System.Object, e As System.Windows.Forms.ScrollEventArgs) Handles VScrollBar5.Scroll
        TextBox5.Text = VScrollBar5.Value

    End Sub

    'Sub Kinect
    '~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
    Private Sub colorready(ByVal sender As Object, ByVal e As ColorImageFrameReadyEventArgs)
        imagez = e.OpenColorImageFrame

    End Sub

    Private Sub skeletonready(ByVal sender As Object, ByVal e As SkeletonFrameReadyEventArgs)
        skeletonz = e.OpenSkeletonFrame

    End Sub

    Private Sub depthready(ByVal sender As Object, ByVal e As DepthImageFrameReadyEventArgs)
        depthz = e.OpenDepthImageFrame

    End Sub

    'Skeleton display
    Public Sub skeletonmethod()

        'Dim G As Graphics
        Dim idindex As Integer
        Dim rad As Single = 3.14 / 180
        Dim antirad As Single = 180 / 3.14

        ' Dim nilaiy As Integer
        'Dim nilaix As Integer
        Dim status1, status2, status3, status4, status5, status6 As String

        Dim skeletons(-1) As Skeleton
        If skeletonz IsNot Nothing Then
            skeletons = New Skeleton(skeletonz.SkeletonArrayLength - 1) {}
            skeletonz.CopySkeletonDataTo(skeletons)
        End If

        Dim penz As Pen = New Pen(Brushes.Gold, 5)
        Dim penz1 As Pen = New Pen(Brushes.Blue, 3)
        Dim penz2 As Pen = New Pen(Brushes.Red, 3)
        Dim myfont As Font = New System.Drawing.Font("Verdana", 20)
        Dim mybrust As Brush = New System.Drawing.SolidBrush(Color.Red)
        Dim mybrust1 As Brush = New System.Drawing.SolidBrush(Color.Green)

        gfx.Clear(Color.Black)

        If skeletons.Length <> 0 Then

            'If kinz.SkeletonStream.AppChoosesSkeletons = False Then kinz.SkeletonStream.AppChoosesSkeletons = True
            'kinz.SkeletonStream.ChooseSkeletons(txttrackingID.Text)
            status1 = "tidak aktif"
            status2 = "tidak aktif"
            status3 = "tidak aktif"
            status4 = "tidak aktif"
            status5 = "tidak aktif"
            status6 = "tidak aktif"

            If RadioButton1.Checked = True Then
                idindex = 1
            ElseIf RadioButton2.Checked = True Then
                idindex = 2
            ElseIf RadioButton3.Checked = True Then
                idindex = 3
            ElseIf RadioButton4.Checked = True Then
                idindex = 4
            ElseIf RadioButton5.Checked = True Then
                idindex = 5
            ElseIf RadioButton6.Checked = True Then
                idindex = 6
            End If

            For Each skel As Skeleton In skeletons

                konter = konter + 1

                If konter = 1 And skel.Position.Z <> 0 Then
                    status1 = "Aktif"
                End If
                If konter = 2 And skel.Position.Z <> 0 Then
                    status2 = "Aktif"
                End If
                If konter = 3 And skel.Position.Z <> 0 Then
                    status3 = "Aktif"
                End If
                If konter = 4 And skel.Position.Z <> 0 Then
                    status4 = "Aktif"
                End If
                If konter = 5 And skel.Position.Z <> 0 Then
                    status5 = "Aktif"
                End If
                If konter = 6 And skel.Position.Z <> 0 Then
                    status6 = "Aktif"
                End If

                'ListBox1.Items.Add(konter & " - " & skel.Position.Z)
                LblStatusTracking.Text = "ID1=" & status1 & "|ID2=" & status2 & "|ID3=" & status3 & "|ID4=" & status4 & "|ID5=" & status5 & "|ID6=" & status6

                'Right Arm
                Dim shoulderright As DepthImagePoint = Me.kinz.CoordinateMapper.MapSkeletonPointToDepthPoint(skel.Joints(JointType.ShoulderRight).Position, DepthImageFormat.Resolution640x480Fps30)
                Dim elbowright As DepthImagePoint = Me.kinz.CoordinateMapper.MapSkeletonPointToDepthPoint(skel.Joints(JointType.ElbowRight).Position, DepthImageFormat.Resolution640x480Fps30)
                Dim wristright As DepthImagePoint = Me.kinz.CoordinateMapper.MapSkeletonPointToDepthPoint(skel.Joints(JointType.WristRight).Position, DepthImageFormat.Resolution640x480Fps30)
                Dim handright As DepthImagePoint = Me.kinz.CoordinateMapper.MapSkeletonPointToDepthPoint(skel.Joints(JointType.HandRight).Position, DepthImageFormat.Resolution640x480Fps30)

                gfx.DrawLine(penz, New Point(shoulderright.X, shoulderright.Y), New Point(elbowright.X, elbowright.Y))
                gfx.DrawLine(penz, New Point(elbowright.X, elbowright.Y), New Point(wristright.X, wristright.Y))
                gfx.DrawLine(penz, New Point(wristright.X, wristright.Y), New Point(handright.X, handright.Y))
                'gfx.DrawRectangle(penz1, New Rectangle(handright.X - 15, handright.Y - 15, 30, 30))

                'Left Arm
                Dim shoulderleft As DepthImagePoint = Me.kinz.CoordinateMapper.MapSkeletonPointToDepthPoint(skel.Joints(JointType.ShoulderLeft).Position, DepthImageFormat.Resolution640x480Fps30)
                Dim elbowleft As DepthImagePoint = Me.kinz.CoordinateMapper.MapSkeletonPointToDepthPoint(skel.Joints(JointType.ElbowLeft).Position, DepthImageFormat.Resolution640x480Fps30)
                Dim wristleft As DepthImagePoint = Me.kinz.CoordinateMapper.MapSkeletonPointToDepthPoint(skel.Joints(JointType.WristLeft).Position, DepthImageFormat.Resolution640x480Fps30)
                Dim handleft As DepthImagePoint = Me.kinz.CoordinateMapper.MapSkeletonPointToDepthPoint(skel.Joints(JointType.HandLeft).Position, DepthImageFormat.Resolution640x480Fps30)

                gfx.DrawLine(penz, New Point(shoulderleft.X, shoulderleft.Y), New Point(elbowleft.X, elbowleft.Y))
                gfx.DrawLine(penz, New Point(elbowleft.X, elbowleft.Y), New Point(wristleft.X, wristleft.Y))
                gfx.DrawLine(penz, New Point(wristleft.X, wristleft.Y), New Point(handleft.X, handleft.Y))
                'gfx.DrawRectangle(penz1, New Rectangle(handleft.X - 15, handleft.Y - 15, 30, 30))

                'Right Leg
                'Dim FootRight As DepthImagePoint = Me.kinz.CoordinateMapper.MapSkeletonPointToDepthPoint(skel.Joints(JointType.FootRight).Position, DepthImageFormat.Resolution640x480Fps30)
                'Dim AnkleRight As DepthImagePoint = Me.kinz.CoordinateMapper.MapSkeletonPointToDepthPoint(skel.Joints(JointType.AnkleRight).Position, DepthImageFormat.Resolution640x480Fps30)
                'Dim KneeRight As DepthImagePoint = Me.kinz.CoordinateMapper.MapSkeletonPointToDepthPoint(skel.Joints(JointType.KneeRight).Position, DepthImageFormat.Resolution640x480Fps30)
                'Dim HipRight As DepthImagePoint = Me.kinz.CoordinateMapper.MapSkeletonPointToDepthPoint(skel.Joints(JointType.HipRight).Position, DepthImageFormat.Resolution640x480Fps30)
                'gfx.DrawLine(penz, New Point(HipRight.X, HipRight.Y), New Point(KneeRight.X, KneeRight.Y))
                'gfx.DrawLine(penz, New Point(KneeRight.X, KneeRight.Y), New Point(AnkleRight.X, AnkleRight.Y))
                'gfx.DrawLine(penz, New Point(AnkleRight.X, AnkleRight.Y), New Point(FootRight.X, FootRight.Y))

                'Left Leg
                'Dim FootLeft As DepthImagePoint = Me.kinz.CoordinateMapper.MapSkeletonPointToDepthPoint(skel.Joints(JointType.FootLeft).Position, DepthImageFormat.Resolution640x480Fps30)
                'Dim AnkleLeft As DepthImagePoint = Me.kinz.CoordinateMapper.MapSkeletonPointToDepthPoint(skel.Joints(JointType.AnkleLeft).Position, DepthImageFormat.Resolution640x480Fps30)
                'Dim KneeLeft As DepthImagePoint = Me.kinz.CoordinateMapper.MapSkeletonPointToDepthPoint(skel.Joints(JointType.KneeLeft).Position, DepthImageFormat.Resolution640x480Fps30)
                'Dim HipLeft As DepthImagePoint = Me.kinz.CoordinateMapper.MapSkeletonPointToDepthPoint(skel.Joints(JointType.HipLeft).Position, DepthImageFormat.Resolution640x480Fps30)
                'gfx.DrawLine(penz, New Point(HipLeft.X, HipLeft.Y), New Point(KneeLeft.X, KneeLeft.Y))
                'gfx.DrawLine(penz, New Point(KneeLeft.X, KneeLeft.Y), New Point(AnkleLeft.X, AnkleLeft.Y))
                'gfx.DrawLine(penz, New Point(AnkleLeft.X, AnkleLeft.Y), New Point(FootLeft.X, FootLeft.Y))

                'Body
                Dim head As DepthImagePoint = Me.kinz.CoordinateMapper.MapSkeletonPointToDepthPoint(skel.Joints(JointType.Head).Position, DepthImageFormat.Resolution640x480Fps30)
                Dim shouldercenter As DepthImagePoint = Me.kinz.CoordinateMapper.MapSkeletonPointToDepthPoint(skel.Joints(JointType.ShoulderCenter).Position, DepthImageFormat.Resolution640x480Fps30)
                Dim Spine As DepthImagePoint = Me.kinz.CoordinateMapper.MapSkeletonPointToDepthPoint(skel.Joints(JointType.Spine).Position, DepthImageFormat.Resolution640x480Fps30)
                Dim HipCenter As DepthImagePoint = Me.kinz.CoordinateMapper.MapSkeletonPointToDepthPoint(skel.Joints(JointType.HipCenter).Position, DepthImageFormat.Resolution640x480Fps30)
                gfx.DrawLine(penz, New Point(head.X, head.Y), New Point(shouldercenter.X, shouldercenter.Y))
                gfx.DrawLine(penz, New Point(shouldercenter.X, shouldercenter.Y), New Point(shoulderright.X, shoulderright.Y))
                gfx.DrawLine(penz, New Point(shouldercenter.X, shouldercenter.Y), New Point(shoulderleft.X, shoulderleft.Y))
                gfx.DrawLine(penz, New Point(shouldercenter.X, shouldercenter.Y), New Point(Spine.X, Spine.Y))
                gfx.DrawLine(penz, New Point(HipCenter.X, HipCenter.Y), New Point(Spine.X, Spine.Y))
                'gfx.DrawLine(penz, New Point(HipCenter.X, HipCenter.Y), New Point(HipRight.X, HipRight.Y))
                'gfx.DrawLine(penz, New Point(HipCenter.X, HipCenter.Y), New Point(HipLeft.X, HipLeft.Y))

                'gfx.DrawRectangle(penz1, New Rectangle(head.X - 15, head.Y - 15, 30, 30))
                'gfx.DrawRectangle(penz1, New Rectangle(shouldercenter.X - 15, shouldercenter.Y - 15, 30, 30))

                'tangankanan = handright.X & "," & handright.Y & "," & handleft.Depth
                'tangankiri = handleft.X & "," & handleft.Y
                'kepala = head.X & "," & head.Y
                'gfx.DrawString(handright.Depth, myfont, mybrust, handright.X, handright.Y)
                'gfx.DrawString(tangankiri, myfont, mybrust, handleft.X, handleft.Y)

                Dim deep_shouldercenter As SkeletonPoint = Me.kinz.CoordinateMapper.MapDepthPointToSkeletonPoint(DepthImageFormat.Resolution640x480Fps30, shouldercenter)
                Dim deep_shoulderright As SkeletonPoint = Me.kinz.CoordinateMapper.MapDepthPointToSkeletonPoint(DepthImageFormat.Resolution640x480Fps30, shoulderright)
                Dim deep_elbowright As SkeletonPoint = Me.kinz.CoordinateMapper.MapDepthPointToSkeletonPoint(DepthImageFormat.Resolution640x480Fps30, elbowright)
                Dim deep_wristright As SkeletonPoint = Me.kinz.CoordinateMapper.MapDepthPointToSkeletonPoint(DepthImageFormat.Resolution640x480Fps30, wristright)
                Dim deep_handright As SkeletonPoint = Me.kinz.CoordinateMapper.MapDepthPointToSkeletonPoint(DepthImageFormat.Resolution640x480Fps30, handright)

                gfx.DrawLine(penz2, New Point(wristright.X, wristright.Y), New Point(shouldercenter.X, shouldercenter.Y))

                If skel.Position.Z <> 0 Then
                    gfx.DrawString(konter, myfont, mybrust, head.X + 10, head.Y)
                    gfx.DrawString((shoulderright.Depth - shoulderleft.Depth), myfont, mybrust, shoulderright.X + 10, shoulderright.Y)
                End If

                If konter = idindex Then

                    bahutengah_x = shouldercenter.X
                    bahutengah_y = shouldercenter.Y
                    bahutengah_z = shouldercenter.Depth
                    bahutengah_dx = deep_shouldercenter.X
                    bahutengah_dy = deep_shouldercenter.Y
                    bahutengah_dz = deep_shouldercenter.Z

                    bahukanan_x = shoulderright.X
                    bahukanan_y = shoulderright.Y
                    bahukanan_z = shoulderright.Depth
                    bahukanan_dx = deep_shoulderright.X * 100
                    bahukanan_dy = deep_shoulderright.Y * 100
                    bahukanan_dz = deep_shoulderright.Z * 100

                    sikukanan_x = elbowright.X
                    sikukanan_y = elbowright.Y
                    sikukanan_z = elbowright.Depth
                    sikukanan_dx = deep_elbowright.X * 100
                    sikukanan_dy = deep_elbowright.Y * 100
                    sikukanan_dz = deep_elbowright.Z * 100

                    pergelangankanan_x = wristright.X
                    pergelangankanan_y = wristright.Y
                    pergelangankanan_z = wristright.Depth
                    pergelangankanan_dx = deep_wristright.X * 100
                    pergelangankanan_dy = deep_wristright.Y * 100
                    pergelangankanan_dz = deep_wristright.Z * 100

                    tangankanan_x = handright.X
                    tangankanan_y = handright.Y

                    olah_sudut_xyz()

                End If

                If konter = 6 Then konter = 0
            Next skel

        End If
    End Sub


    Private Sub olah_sudut_xyz()
        Txtpundak_x.Text = bahukanan_dx
        TxtPundak_y.Text = bahukanan_dy
        txtpundak_z.Text = bahukanan_dz

        TxtSiku_x.Text = sikukanan_dx
        TxtSiku_y.Text = sikukanan_dy
        Txtsiku_z.Text = sikukanan_dz

        TxtGelang_x.Text = pergelangankanan_dx
        TxtGelang_y.Text = pergelangankanan_dy
        TxtGelang_z.Text = pergelangankanan_dz

        '-----------------------------------
        'a4 = a4 * (180 / 3.14)
        Dim sudut1, sudut2, sudut3, sudut4 As Single
        'Dim status_persamaan As String = "persamaan ..."

        '        LblStatus_Persamaan.Text = status_persamaan
        '----------------------------------------------------------
        'rumus baru
        Dim X_LB As Single = pergelangankanan_dx - sikukanan_dx
        Dim Y_LB As Single = sikukanan_dy - pergelangankanan_dy
        Dim Z_LB As Single = sikukanan_dz - pergelangankanan_dz

        Dim X_LA As Single = sikukanan_dx - (bahukanan_dx + 7)
        Dim Y_LA As Single = bahukanan_dy - sikukanan_dy
        Dim Z_LA As Single = bahukanan_dz - sikukanan_dz


        Txt_XLA.Text = X_LA
        Txt_YLA.Text = Y_LA
        Txt_ZLA.Text = Z_LA

        Txt_XLB.Text = X_LB
        Txt_YLB.Text = Y_LB
        Txt_ZLB.Text = Z_LB

        Dim XY_LA As Single = Sqrt(X_LA ^ 2 + Y_LA ^ 2)
        Dim ZY_LA As Single = Sqrt(Z_LA ^ 2 + Y_LA ^ 2)

        Dim A1N As Single = Atan(Z_LA / Y_LA) * 180 / PI
        Dim A2N As Single = Atan(X_LA / Y_LA) * 180 / PI

        Dim A1 As Single = Atan(Z_LA / XY_LA) * 180 / PI
        Dim A2 As Single = Atan(X_LA / ZY_LA) * 180 / PI

        Dim AI2 As Single = Atan(X_LB / Y_LB) * 180 / PI

        Dim LB As Single = Sqrt(X_LB ^ 2 + Y_LB ^ 2 + Z_LB ^ 2)
        Dim LA As Single = Sqrt(X_LA ^ 2 + Y_LA ^ 2 + Z_LA ^ 2)

        Dim X_LI1 As Single = X_LA - X_LB
        Dim Y_LI1 As Single = Y_LB - Y_LA
        Dim Z_LI1 As Single = Z_LB - Z_LA

        Dim LI1 As Single = Sqrt(X_LI1 ^ 2 + Y_LI1 ^ 2 + Z_LI1 ^ 2)

        Dim cosA4 As Single = (LA ^ 2 + LB ^ 2 - LI1 ^ 2) / (2 * LA * LB)
        Dim A4 As Single = Acos(cosA4) * 180 / PI
        Dim A3 As Single

        If A4 <= 25 Or (AI2 > A2N - 4 And AI2 < A2N + 4) Then
            A3 = 0
        Else

            Dim LI3 As Single = Sin(A4 * PI / 180) * LB
            Dim LI4 As Single = Cos((A1 + A4) * PI / 180) * LB

            Dim Z_LI2 As Single = Sin((A1 + A4) * PI / 180) * LB
            Dim X_LI2 As Single = Sin(A2N * PI / 180) * LI4
            Dim Y_LI2 As Single = Cos(A2N * PI / 180) * LI4
            '
            Dim X_LI5 As Single = X_LI2 - X_LB
            Dim Y_LI5 As Single = Y_LI2 - Y_LB
            Dim Z_LI5 As Single = Z_LI2 - Z_LB
            '
            Dim LI5 As Single = Sqrt(X_LI5 ^ 2 + Y_LI5 ^ 2 + Z_LI5 ^ 2)
            Dim cosA3 As Single = (LI3 ^ 2 + LI3 ^ 2 - LI5 ^ 2) / (2 * LI3 * LI3)
            A3 = Acos(cosA3) * 180 / PI

            If (AI2 <= A2N - 4 And Y_LB >= 0) Or (AI2 >= A2N + 4 And Y_LB < 0) Then
                A3 *= -1
            End If

            'If A3 = last_a3 Then
            ' A3 *= -1
            'End If

        End If


        '
        '...............
        'dari sini
        'Dim X_LBu As Single = X_LB
        'Dim Y_LBu As Single = Y_LB

        'If X_LBu < 0 Then X_LBu *= -1
        'If Y_LBu < 0 Then Y_LBu *= -1

        'Dim A3in As Single = Atan(X_LB / Y_LB) * 180 / PI
        'Dim A3 As Single
        '
        'If A3in > (A2N - 16) - 3 And A3in < (A2N - 16) + 3 Then
        ' A3 = 0
        ' Else
        'If A3in > 0 Then
        'A3 = A3in - (A2N - 16)
        'Else
        'A3 = A3in + 16
        'End If

        'End If
        'sampai sini
        '....................

        If A1N < 0 Then
            If Z_LA < 0 Then A1N = 0
            If Y_LA < 0 Then A1N = 90
        End If

        If A2 < 0 Then A2 = 0
        If A3 < -45 Then A3 = -45

        If A4 < 0 Then A4 = 0
        If A1N > 90 Then A1N = 90
        If A2 > 90 Then A2 = 90
        If A3 > 45 Then A3 = 45
        If A4 > 90 Then A4 = 90
        'last_a3 = A3 * -1

        sudut1 = A1N
        sudut2 = A2
        sudut3 = A3
        sudut4 = A4
        '---------------------------------------------------------

        If Single.IsNaN(sudut1) = True Then
            sudut1 = 0
        Else
            sudut1 = Round(sudut1)
        End If

        If Single.IsNaN(sudut2) = True Then
            sudut2 = 0
        Else
            sudut2 = Round(sudut2)
        End If

        If Single.IsNaN(sudut3) = True Then
            sudut3 = 0
        Else
            sudut3 = Round(sudut3)
        End If

        If Single.IsNaN(sudut4) = True Then
            sudut4 = 0
        Else
            sudut4 = Round(sudut4)
        End If

        '-----------------------------------
        lservo1 = (1024 / 90 * sudut1) + 2048
        lservo2 = (1024 / 90 * sudut2) + 2048

        lservo3 = (153 / 45 * sudut3) + 512
        If lservo3 > 665 Then lservo3 = 665
        If lservo3 < 359 Then lservo3 = 359

        lservo4 = (308 / 90 * sudut4) + 512

        'lastservo1 = dxl_read_word(1, 36)
        'lastservo2 = dxl_read_word(2, 36)

        'If lservo3 < 0 Then lservo3 = 0
        'If lservo3 > 1023 Then lservo3 = 1023
        If lservo4 < 512 Then lservo4 = 512
        If lservo4 > 820 Then lservo4 = 820

        txtsudut_a1.Text = sudut1
        txtsudut_a2.Text = sudut2
        Txtsudut_a3.Text = sudut3
        txtsudut_a4.Text = sudut4

        TextBox5.Text = lservo1
        VScrollBar5.Value = lservo1
        TextBox6.Text = lservo2
        VScrollBar6.Value = lservo2
        TextBox7.Text = lservo3
        VScrollBar7.Value = lservo3
        TextBox8.Text = lservo4
        VScrollBar8.Value = lservo4

    End Sub


    Private Sub TimerKinect_Tick(sender As System.Object, e As System.EventArgs) Handles TimerKinect.Tick
        skeletonmethod()
        PictB_Kinect.Image = piccolor

    End Sub



    Private Sub TimerKirimData_Tick(sender As System.Object, e As System.EventArgs) Handles TimerKirimData.Tick

        lastservo1 = VScrollBar1.Value

        dout1 = (khighpass * lastservo1) + (klowpass * lservo1)

        If dout1 < 2048 Then dout1 = 2048
        If dout1 > 3072 Then dout1 = 3072

        nservo1 = dout1

        TextBox1.Text = nservo1
        VScrollBar1.Value = nservo1
        'dxl_write_word(1, P_GOAL_POSITION_L, nservo1)

        '-------------------------------------------------------------
        lastservo2 = VScrollBar2.Value

        dout2 = (khighpass * lastservo2) + (klowpass * lservo2)

        If dout2 < 2048 Then dout2 = 2048
        If dout2 > 3072 Then dout2 = 3072

        nservo2 = dout2

        TextBox2.Text = Str(nservo2) & "|" & Str(skalaplusb)
        VScrollBar2.Value = nservo2
        'dxl_write_word(2, P_GOAL_POSITION_L, nservo2)
        '-----------------------------------------------------------
        lastservo3 = VScrollBar3.Value
        dout3 = (khighpass2 * lastservo3) + (klowpass2 * lservo3)

        If dout3 < 250 Then dout3 = 250
        If dout3 > 820 Then dout3 = 820

        nservo3 = dout3

        TextBox3.Text = Str(nservo3) '& "|" & Str(skalaplusb)
        VScrollBar3.Value = nservo3
        'dxl_write_word(3, P_GOAL_POSITION_L, nservo3)

        '----------------------------------------
        lastservo4 = VScrollBar4.Value
        dout4 = (khighpass2 * lastservo4) + (klowpass2 * lservo4)

        If dout4 < 512 Then dout4 = 512
        If dout4 > 820 Then dout4 = 820

        nservo4 = dout4

        TextBox4.Text = Str(nservo4) '& "|" & Str(skalaplusb)
        VScrollBar4.Value = nservo4
        'dxl_write_word(4, P_GOAL_POSITION_L, nservo4)

        Dim datakirimlan As String = "K" & nservo1 & "L" & nservo2 & "M" & nservo3 & "N" & nservo4 & "O" '& Form2.TextBox1.Text & "P"
        Dim datasiapkirim() As Byte = Encoding.ASCII.GetBytes(datakirimlan)
        sendingClient.Send(datasiapkirim, datasiapkirim.Length)

    End Sub


    Private Sub Button1_Click_1(sender As System.Object, e As System.EventArgs) Handles Button1.Click
        'Me.kinz.SkeletonStream.Enable()
        Me.kinz.SkeletonStream.Enable(smooth)

        Me.kinz.DepthStream.Enable(DepthImageFormat.Resolution640x480Fps30)
        'AddHandler Me.kinz.ColorFrameReady, AddressOf colorready
        AddHandler Me.kinz.DepthFrameReady, AddressOf depthready
        AddHandler Me.kinz.SkeletonFrameReady, AddressOf skeletonready


        Me.kinz.Start()
        Me.kinz.SkeletonStream.TrackingMode = SkeletonTrackingMode.Seated
        Me.kinz.ElevationAngle = 0
        TimerKinect.Enabled = True
        'aktifkan ini
        '-----------
     
    End Sub

    Private Sub Button2_Click(sender As System.Object, e As System.EventArgs) Handles Button2.Click
        TimerKinect.Enabled = False
        TimerKirimData.Enabled = False
        'TimerDepth.Enabled = False
        Me.kinz.Stop()

    End Sub


    Private Sub Timer1_Tick(sender As System.Object, e As System.EventArgs) Handles Timer1.Tick
        Dim datasiapkirim() As Byte = Encoding.ASCII.GetBytes(Form2.TextBox1.Text)
        sendingClient.Send(datasiapkirim, datasiapkirim.Length)
    End Sub

    Private Sub Button5_Click(sender As System.Object, e As System.EventArgs) Handles Button5.Click
        Form2.Show()

    End Sub

    Private Sub Button4_Click(sender As System.Object, e As System.EventArgs) Handles Button4.Click
        Timer1.Enabled = True

    End Sub
End Class

Imports System
Imports System.IO.Ports
Imports System.Windows.Forms
Imports System.Net
Imports System.Net.Sockets
Imports System.Threading
Imports System.Text

Public Class Form1
    Inherits Form

    Dim nilai, nilai2, nilai3 As Integer
    Dim keep1, keep2, keep3 As Integer

    Dim status As Integer = 0
    Dim langkah As Integer = 1

    Dim nservo1 As Integer = 2100
    Dim nservo2 As Integer = 2250
    Dim nservo3 As Integer = 512
    Dim nservo4 As Integer = 512

    Dim konter As Integer = 0

    Dim nsudut1 As Integer = 2100
    Dim nsudut2 As Integer = 2250
    Dim nsudut3 As Integer = 512
    Dim nsudut4 As Integer = 512
    Dim ntunggu As Integer = 0
    Dim gpservo1, gpservo2, gpservo3, gpservo4 As Integer

    Delegate Sub addMessage(ByRef message As String)
    Private username As String = "BuTO client"
    Private Const port As Integer = 2390
    Private Const broadCastAddress As String = "192.168.0.104"

    Private receivingClient As UdpClient
    Private sendingClient As UdpClient
    Private receivingThread As Thread

    Private Sub initializeSender()
        sendingClient = New UdpClient(broadCastAddress, port)
        sendingClient.EnableBroadcast = True

    End Sub

    Private Sub initializeReceiver()
        receivingClient = New UdpClient(port)

        If (String.IsNullOrEmpty(port)) Then
            MessageBox.Show("port is not available")
        End If
        Dim start As ThreadStart = New ThreadStart(AddressOf receiver)
        receivingThread = New Thread(start)
        receivingThread.IsBackground = True
        receivingThread.Start()

    End Sub


    Private Sub receiver()
        Dim endpoint As IPEndPoint = New IPEndPoint(IPAddress.Any, port)
        Dim messageDelegate As addMessage = AddressOf MessageReceived

        While (True)
            Dim Data() As Byte
            Data = receivingClient.Receive(endpoint)
            Dim Message As String = Encoding.ASCII.GetString(Data)
            Invoke(messageDelegate, Message)

        End While

    End Sub


    Private Sub MessageReceived(ByRef message As String)
        TextBox1.Text = ""
        TextBox1.Text += message

        'Dim data() As Byte = Encoding.ASCII.GetBytes(TextBox1.Text)
        'sendingClient.Send(data, data.Length)

    End Sub


    Private Sub Form1_Load(sender As System.Object, e As System.EventArgs) Handles MyBase.Load

        initializeSender()
        initializeReceiver()

    End Sub


    Private Sub Form1_Shown(sender As System.Object, e As System.EventArgs) Handles MyBase.Shown

    End Sub


    Private Sub Button1_Click(sender As System.Object, e As System.EventArgs) Handles Btn_BukaPort.Click
        If dxl_initialize(12, 1) = 1 Then
            'Set goal speed	
            dxl_write_word(1, P_GOAL_SPEED_L, 180) '270
            dxl_write_word(2, P_GOAL_SPEED_L, 180) '270
            dxl_write_word(3, P_GOAL_SPEED_L, 270) '270
            dxl_write_word(4, P_GOAL_SPEED_L, 270) '270

            'Set goal position
            dxl_write_word(1, P_GOAL_POSITION_L, 2100)
            dxl_write_word(2, P_GOAL_POSITION_L, 2250)
            dxl_write_word(3, P_GOAL_POSITION_L, 512)
            dxl_write_word(4, P_GOAL_POSITION_L, 512)
            'dxl_write_word(5, P_GOAL_POSITION_L, 512)
        Else
            MsgBox("cek koneksi")
        End If

        'SerialPort1.PortName = "COM8"         'Set SerialPort1 to the selected COM port at startup
        'SerialPort1.BaudRate = 9600
        'SerialPort1.Parity = IO.Ports.Parity.None
        'SerialPort1.StopBits = IO.Ports.StopBits.One
        'SerialPort1.DataBits = 8            'Open our serial port


        If SerialPort1.IsOpen = False Then
            SerialPort1.Open()
            'MsgBox("Cek koneksi", vbCritical)
        End If

    End Sub


    Private Sub VScrollBar1_Scroll(sender As System.Object, e As System.Windows.Forms.ScrollEventArgs) Handles ScrBar_BitServo1.Scroll
        Txt_BitServo1.Text = Str(ScrBar_BitServo1.Value)

        nservo1 = ScrBar_BitServo1.Value
        nsudut1 = nservo1 / 4095 * 360

        dxl_write_word(1, P_GOAL_POSITION_L, nservo1)

        Txt_BitServo1.Text = nservo1
        Txt_DegServo1.Text = nsudut1

    End Sub


    Private Sub VScrollBar2_Scroll(sender As System.Object, e As System.Windows.Forms.ScrollEventArgs) Handles ScrBar_BitServo2.Scroll
        Txt_BitServo2.Text = Str(ScrBar_BitServo2.Value)

        nservo2 = ScrBar_BitServo2.Value
        nsudut2 = nservo2 / 4095 * 360

        dxl_write_word(2, P_GOAL_POSITION_L, nservo2)

        Txt_BitServo2.Text = nservo2
        Txt_DegServo2.Text = nsudut2

    End Sub


    Private Sub VScrollBar3_Scroll(sender As System.Object, e As System.Windows.Forms.ScrollEventArgs) Handles ScrBar_BitServo3.Scroll
        Txt_BitServo3.Text = Str(ScrBar_BitServo3.Value)

        nservo3 = ScrBar_BitServo3.Value
        nsudut3 = nservo3 / 1023 * 300

        dxl_write_word(3, P_GOAL_POSITION_L, nservo3)

        Txt_BitServo3.Text = nservo3
        Txt_DegServo3.Text = nsudut3

    End Sub


    Private Sub VScrollBar4_Scroll(sender As System.Object, e As System.Windows.Forms.ScrollEventArgs) Handles ScrBar_BitServo4.Scroll
        Txt_BitServo4.Text = Str(ScrBar_BitServo4.Value)

        nservo4 = ScrBar_BitServo4.Value
        nsudut4 = nservo4 / 1023 * 300

        dxl_write_word(4, P_GOAL_POSITION_L, nservo4)

        Txt_BitServo4.Text = nservo4
        Txt_DegServo4.Text = nsudut4

    End Sub


    Private Sub Button1_Click_1(sender As System.Object, e As System.EventArgs) Handles Button1.Click
        Txt_GPServo1.Text = ""
        Txt_GPServo2.Text = ""
        Txt_GPServo3.Text = ""
        Txt_GPServo4.Text = ""

        Timerplay.Enabled = True
        Button1.Enabled = False

    End Sub

    Private Sub Timerplay_Tick(sender As System.Object, e As System.EventArgs) Handles Timerplay.Tick
        'tulis nilai pulsa
        
        'baca posisi servo
        If konter = 0 Or konter = 5 Or konter = 10 Or konter = 15 Then
            gpservo1 = dxl_read_word(1, 36)
            gpservo2 = dxl_read_word(2, 36)
            gpservo3 = dxl_read_word(3, 36)
            gpservo4 = dxl_read_word(4, 36)

            Txt_GPServo1.Text = Txt_GPServo1.Text & gpservo1 & "; "
            Txt_GPServo2.Text = Txt_GPServo2.Text & gpservo2 & "; "
            Txt_GPServo3.Text = Txt_GPServo3.Text & gpservo3 & "; "
            Txt_GPServo4.Text = Txt_GPServo4.Text & gpservo4 & "; "

        End If

        konter = konter + 1
        If konter > 17 Then
            Button1.Enabled = True
            konter = 0
            Timerplay.Enabled = False

        End If

    End Sub

    Private Sub Btn_Keluar_Click(sender As System.Object, e As System.EventArgs) Handles Btn_Keluar.Click
        Close()

    End Sub

    Private Sub Timer1_Tick(sender As System.Object, e As System.EventArgs) Handles Timer1.Tick
        Dim nK As Integer = TextBox1.Text.IndexOf("K")
        Dim nL As Integer = TextBox1.Text.IndexOf("L")
        Dim nM As Integer = TextBox1.Text.IndexOf("M")
        Dim nN As Integer = TextBox1.Text.IndexOf("N")
        Dim nO As Integer = TextBox1.Text.IndexOf("O")
        Dim nS As Integer = TextBox1.Text.IndexOf("S")

        Dim sServo1 As String = TextBox1.Text.Substring(nK + 1, nL - nK - 1)
        Dim sServo2 As String = TextBox1.Text.Substring(nL + 1, nM - nL - 1)
        Dim sServo3 As String = TextBox1.Text.Substring(nM + 1, nN - nM - 1)
        Dim sServo4 As String = TextBox1.Text.Substring(nN + 1, nO - nN - 1)
        Dim sMikro As String = TextBox1.Text.Substring(nO, nS - nO + 1)

        TextBox2.Text = sServo1 & ";" & sServo2 & ";" & sServo3 & ";" & sServo4 & ";"
        TextBox3.Text = sMikro

        If SerialPort1.IsOpen = True Then
            ScrBar_BitServo1.Value = nservo1
            dxl_write_word(1, P_GOAL_POSITION_L, sServo1)

            ScrBar_BitServo2.Value = nservo2
            dxl_write_word(2, P_GOAL_POSITION_L, sServo2)

            ScrBar_BitServo3.Value = nservo3
            dxl_write_word(3, P_GOAL_POSITION_L, sServo3)

            ScrBar_BitServo4.Value = nservo4
            dxl_write_word(4, P_GOAL_POSITION_L, sServo4)

            SerialPort1.WriteLine(sMikro + Chr(13))

        End If
        
    End Sub

    Private Sub Button2_Click(sender As System.Object, e As System.EventArgs) Handles Button2.Click
        Timer1.Enabled = True
        
        'Process.Start("C:\Users\Syamsiar Kautsar\Music\To MAsSEm\Voice 9.wav")
        'Process.Start("D:\Kuliah\S2\Thesis prepare\Presentasi Sidang\Presentasi Seminar (Slide Show).ppsx")

    End Sub

    Private Sub Timer2_Tick(sender As System.Object, e As System.EventArgs)

    End Sub
End Class