Imports System.Math
Imports System.Threading
Imports System.Drawing
Imports System.Windows.Forms
Imports System.IO.Ports


Partial Public Class frmArm



    Delegate Sub DelegateArmLoad()
    Public Sub ftnArmLoad() 'Handles MyBase.Load
        Try
            If Me.InvokeRequired Then
                Dim newDelegate As New DelegateArmLoad(AddressOf ftnArmLoad)
                Me.Invoke(newDelegate, New Object() {})
            Else
                Me.Visible = True
                strStatusDriving = "Starting Arm "
                boolLocationFocused = False
                updnComPort.Value = intArmComPort
                UnSafeNativeMethods.dxl_initialize(intArmComPort, constDefaultBaudNum)
                subInitializeDynamixels()
                UpDnObject.Value = 1
                textboxName.Text = taoObjectArray(1).strName
                updnX.Value = taoObjectArray(1).tmcLocation.X
                updnY.Value = taoObjectArray(1).tmcLocation.Y
                updnOrientation.Value = taoObjectArray(1).intOrientation
                updnHeight.Value = taoObjectArray(1).intObjectHeight - tjcJointCoordinates.tpcShoulderBase.Z
                updnRotation.Value = taoObjectArray(UpDnObject.Value).intRotation
                upDnGripOffset.Value = taoObjectArray(UpDnObject.Value).intGripOffset_mm
                boolStop = False
                taaArmAngles = ftnGetArmAngles()
                tjcJointCoordinates = ftnForwardKinematicsCoordinates(taaArmAngles)
                UpDnObject_ValueChanged()
                timerArmWatchDog.Enabled = True
                timerArmUpdate.Enabled = True
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In ftnArmLoad :  " & ex.ToString)
        End Try
    End Sub

    Public Sub subInitializeDynamixels()
        Try
            If (UnSafeNativeMethods.dxl_initialize(intArmComPort, constDefaultBaudNum) = 0) Then
                boolArmConnected = False
                taaArmAngles.sngShoulder = 114
                taaArmAngles.sngElbow = 0
                taaArmAngles.sngWrist = 0   '
                taaArmAngles.sngWristRotate = 0
                taaArmAngles.sngGripperOpening = ftnPositionToDegree(constGripperOpenPosition)
            ElseIf Not boolShutDown Then
                boolArmConnected = True
                Dim portArm As New SerialPort("COM" & intArmComPort.ToString)
                portArm.BaudRate = 115200
                UnSafeNativeMethods.dxl_write_word(254, constMovingSpeed, 50)  '  
                'dxl_write_word(intServoID, constMovingSpeed + 1, 0)  '  
                UnSafeNativeMethods.dxl_write_word(254, constHiTemperature, 55)  '  
                UnSafeNativeMethods.dxl_write_byte(254, constReturnDelayTime, 0)  '  
                UnSafeNativeMethods.dxl_write_byte(254, constCWComplianceMargin, 3)  '  
                UnSafeNativeMethods.dxl_write_byte(254, constCCWComplianceMargin, 3)  '  
                UnSafeNativeMethods.dxl_write_word(254, constCWComplianceSlope, 3)  ' 
                UnSafeNativeMethods.dxl_write_word(254, constCCWComplianceSlope, 3)  ' 
                UnSafeNativeMethods.dxl_write_word(254, constTorqueLimit, 1023)  '  
                UnSafeNativeMethods.dxl_write_word(254, constTorqueEnable, 1)  '  
            End If
            taaArmAngles = ftnGetArmAngles()
            tjcJointCoordinates = ftnForwardKinematicsCoordinates(taaArmAngles)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In subInitializeDynamixels :  " & ex.ToString)
        End Try
    End Sub

    Dim boolAlreadyClosed As Boolean = False
    Delegate Sub DelegateArmExit()
    Public Sub subArmExit() Handles Me.FormClosing
        Try
            If Me.InvokeRequired And Not boolAlreadyClosed Then
                Dim newDelegate As New DelegateArmExit(AddressOf subArmExit)
                Me.Invoke(newDelegate, New Object() {})
            ElseIf Not boolAlreadyClosed Then
                boolLocationFocused = False
                boolAlreadyClosed = True
                subHome(True)
                UnSafeNativeMethods.dxl_write_word(254, constTorqueLimit, 1)  '  
                UnSafeNativeMethods.dxl_write_word(254, constTorqueEnable, 0)  '  
                UnSafeNativeMethods.dxl_terminate()
                timerArmWatchDog.Enabled = False
                timerArmUpdate.Enabled = False
                boolEnableArm = False
                Me.Close()
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In subArmExit :  " & ex.ToString)
        End Try
    End Sub

    ' =================================   MANUAL CONTROL ========================================
    Private Sub subSegment_MouseWheel(ByVal sender As System.Object, ByVal e As System.Windows.Forms.MouseEventArgs) Handles Me.MouseWheel  '
        Try
            boolStop = False
            boolLocationFocused = False
            Select Case intArmSegmentSelect
                Case constShoulderSegment
                    taaArmAngles.sngShoulder += e.Delta / 150
                    taaArmAngles.sngShoulder = ftnConvertTo180(taaArmAngles.sngShoulder)
                    taaArmAngles.sngShoulder = Max(Min(taaArmAngles.sngShoulder, constMaxJointAngle), -70)
                Case constElbowSegment
                    taaArmAngles.sngElbow += e.Delta / 150
                    taaArmAngles.sngElbow = ftnConvertTo180(taaArmAngles.sngElbow)
                    taaArmAngles.sngElbow = Max(Min(taaArmAngles.sngElbow, (constMaxJointAngle + taaArmAngles.sngShoulder)), (-constMaxJointAngle + taaArmAngles.sngShoulder))
                Case constWristRotateSegment
                    taaArmAngles.sngWristRotate += e.Delta / 150
                    taaArmAngles.sngWristRotate = ftnConvertTo180(taaArmAngles.sngWristRotate)
                    taaArmAngles.sngWristRotate = Max(Min(taaArmAngles.sngWristRotate, 150), -150)
                Case constWristSegment
                Case constGripperSegment
                    Dim intClosedDegrees As Integer = ftnPositionToDegree(constGripperClosedPosition)
                    Dim intOpenDegrees As Integer = ftnPositionToDegree(constGripperOpenPosition)
                    taaArmAngles.sngGripperOpening += e.Delta / 150
                    taaArmAngles.sngGripperOpening = Min(Max(taaArmAngles.sngGripperOpening, intOpenDegrees), intClosedDegrees)
            End Select
            subMoveArmToNewAngles(taaArmAngles, False)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In subSegment_MouseWheel :  " & ex.ToString)
        End Try
    End Sub


    Dim boolMouseDown As Boolean = False
    Private Sub subArm_MouseDown(ByVal sender As System.Object, ByVal e As System.Windows.Forms.MouseEventArgs) Handles pbxArm.MouseDown
        Try
            boolLocationFocused = False
            boolMouseDown = True
            If (e.Button = System.Windows.Forms.MouseButtons.Right) Then  ' Select the next arm segment
                intArmSegmentSelect += 1
                If (intArmSegmentSelect > 6) Then intArmSegmentSelect = 1
            ElseIf (e.Button = System.Windows.Forms.MouseButtons.Left) Then  ' Set a destination
                boolStop = False
                Dim tpcDestination As typePointCoordinate = ftnMousePointCoordinate()
                If boolDebugArm Then frmDebug.subWriteLogs(" subArm_MouseDown : tpcDestination = " & tpcDestination.X & ", " & tpcDestination.Y & ", " & tpcDestination.Z)
                subInitializeDynamixels()
                ftnPlaceGripperAtPoint(tpcDestination, False)
            ElseIf (e.Button = System.Windows.Forms.MouseButtons.Middle) Then  ' Point gripper at point
                boolStop = False
                Dim tpcDestination As typePointCoordinate = ftnMousePointCoordinate()
                ftnAimGripperAtPoint(tpcDestination)
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subArm_MouseDown :  " & ex.ToString)
        End Try
    End Sub

    Private Sub subArm_MouseUp() Handles pbxArm.MouseUp
        On Error Resume Next
        boolMouseDown = False
    End Sub

    Private Sub subArm_MouseMove() Handles pbxArm.MouseMove  'base, shoulder, elbow, wrist control
        On Error Resume Next
        Dim tpcDestination As typePointCoordinate = ftnMousePointCoordinate()
        tsslXY.Text = tpcDestination.Y & ", " & tpcDestination.Z
    End Sub

    Function ftnMousePointCoordinate() As typePointCoordinate
        Try
            ftnMousePointCoordinate.X = tjcJointCoordinates.tpcShoulderBase.X
            ftnMousePointCoordinate.Y = (-pbxArm.Width * 0.25 + (MousePosition.X - Me.Location.X - pbxArm.Location.X - pbxArm.Padding.Left - pbxArm.Margin.Left - 4.5)) * 2 + tjcJointCoordinates.tpcShoulderBase.Y
            ftnMousePointCoordinate.Z = (pbxArm.Height * 0.5 - (MousePosition.Y - Me.Location.Y - pbxArm.Location.Y - pbxArm.Padding.Top - pbxArm.Margin.Top - 23)) * 2 + tjcJointCoordinates.tpcShoulderBase.Z
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In ftnMousePointCoordinate :  " & ex.ToString)
        End Try
    End Function

    ' ============================Timers==========================================

    Private Sub timerWatchDog_Tick() Handles timerArmWatchDog.Tick
        Try
            If Me.Visible And boolArmConnected And Not boolMoveArmToNewAnglesBusy And timerArmWatchDog.Enabled And timerArmUpdate.Enabled And Not InvokeRequired Then
                timerArmWatchDog.Enabled = False
                Dim boolOK As Boolean = False
                If Not boolSimulatedMovement Then
                    For intServoID As Integer = 2 To 7 ' check that USB2Dynamixel still working
                        UnSafeNativeMethods.dxl_ping(intServoID)
                        If (UnSafeNativeMethods.dxl_get_result() = COMM_RXSUCCESS) Then
                            boolOK = True
                            Exit For
                        End If
                    Next intServoID
                    If Not boolOK Then subInitializeDynamixels()
                    For intServoID As Integer = 2 To 7 ' check for overload, overtemperature
                        UnSafeNativeMethods.dxl_ping(intServoID)
                        If (timeJointMove(intServoID).AddSeconds(8) < Now()) Then
                            If (Abs(ftnGetDynaLoad(intServoID)) > 200) Then _
                                UnSafeNativeMethods.dxl_write_word(intServoID, constGoalPosition, UnSafeNativeMethods.dxl_read_word(intServoID, constPresentPosition))
                        End If
                        If (UnSafeNativeMethods.dxl_get_rxpacket_error(ERRBIT_OVERLOAD) = 1) Or (UnSafeNativeMethods.dxl_get_rxpacket_error(ERRBIT_OVERHEAT) = 1) Then
                            UnSafeNativeMethods.dxl_write_word(intServoID, constGoalPosition, UnSafeNativeMethods.dxl_read_word(intServoID, constPresentPosition))
                            subHighlightDynamixelError(intServoID)
                        End If
                    Next intServoID
                End If
            End If
            timerArmWatchDog.Enabled = Not boolShutDown
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in timerWatchDog_Tick :  " & ex.ToString)
        End Try
    End Sub

    Dim boolTorqueEnabled As Boolean
    Private Sub subTimerUpdate() Handles timerArmUpdate.Tick
        Try
            If Me.Visible And Not boolMoveArmToNewAnglesBusy And timerArmUpdate.Enabled And timerArmWatchDog.Enabled And Not InvokeRequired Then
                timerArmUpdate.Enabled = False
                Dim timeOut As DateTime = Now().AddSeconds(-90)
                Dim boolArmInactive As Boolean = (frmArm.timeJointMove(2) < timeOut) And (frmArm.timeJointMove(4) < timeOut) And _
                    (frmArm.timeJointMove(6) < timeOut) And (frmArm.timeJointMove(7) < timeOut)
                If boolDrivingToDestination Then
                    If (frmArm.taaArmAngles.sngGripperOpening < (frmArm.constGripperOpenPosition + 35)) Then
                        UnSafeNativeMethods.dxl_write_word(254, frmArm.constTorqueLimit, 100) ' Protect battery from overcurrent due to arm colliding with wall
                        frmArm.timeJointMove(2) = Now()
                        boolTorqueEnabled = False
                    End If
                ElseIf boolTorqueEnabled And boolArmInactive Then
                    UnSafeNativeMethods.dxl_write_word(254, frmArm.constTorqueLimit, 1)  ' 
                    boolTorqueEnabled = False
                ElseIf Not boolTorqueEnabled And Not boolArmInactive Then
                    UnSafeNativeMethods.dxl_write_word(254, frmArm.constTorqueLimit, 1023) ' 
                    boolTorqueEnabled = True
                End If
                Dim taaActualArmAngles As typeArmAngles = ftnGetArmAngles()
                subDisplayDynaParameters()
                subArmApiControl()
                If boolArmConnected Then
                    tsslArmStatus.ForeColor = Color.Green
                    tsslArmStatus.Text = "Operating"
                Else
                    tsslArmStatus.ForeColor = Color.Red
                    tsslArmStatus.Text = "Simulated"
                End If
                Select Case intArmSegmentSelect
                    Case constShoulderSegment
                        tsslJoint.Text = "Shoulder"
                        tsslAngles.Text = taaActualArmAngles.sngShoulder.ToString
                    Case constElbowSegment
                        tsslJoint.Text = "Elbow"
                        tsslAngles.Text = taaActualArmAngles.sngElbow.ToString
                    Case constWristRotateSegment
                        tsslJoint.Text = "Wrist Rotate"
                        tsslAngles.Text = taaActualArmAngles.sngWristRotate.ToString
                    Case constWristSegment
                        tsslJoint.Text = "Wrist"
                        tsslAngles.Text = taaActualArmAngles.sngWrist.ToString
                    Case constGripperSegment
                        tsslJoint.Text = "Gripper"
                        tsslAngles.Text = taaActualArmAngles.sngGripperOpening.ToString
                    Case Else
                        tsslJoint.Text = "-----"
                End Select
                taaArmAngles = taaActualArmAngles
                tjcJointCoordinates = ftnForwardKinematicsCoordinates(taaActualArmAngles)
                subDrawArm()
                tsslPosition.Text = tjcJointCoordinates.tpcGripperEnd.X & ",  " & tjcJointCoordinates.tpcGripperEnd.Y & ",  " & tjcJointCoordinates.tpcGripperEnd.Z
                If boolShutDown Then subArmExit()
                timerArmUpdate.Enabled = Not boolShutDown
            End If
        Catch ex As Exception
            timerArmUpdate.Enabled = Not boolShutDown
            frmDebug.subWriteLogs(" EXCEPTION in subTimerUpdate :  " & ex.ToString)
        End Try
    End Sub


    Function ftnGetDynaLoad(ByRef intServoID As Integer) As Integer
        Try
            ftnGetDynaLoad = UnSafeNativeMethods.dxl_read_word(intServoID, constLoad)
            If (ftnGetDynaLoad > 1023) Then ftnGetDynaLoad = -(ftnGetDynaLoad - 1024)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnGetDynaLoad :  " & ex.ToString)
        End Try
    End Function

    ' ==========================================  Debug Info  ============================

    Sub subDisplayDynaParameters()
        Try
            Dim strText As String = "Joint    | Goal  Actual Angle  Load  Temp"
            strText &= vbCrLf & "=========================================="
            Dim intPosition, intGoal, intLoad, intAngle, intTemp As Integer
            Dim strID As String = ""
            For intServoID As Integer = 2 To 7  ' 
                Select Case intServoID
                    Case constBaseServo
                        strID = "Base |"
                    Case constShoulderRServo
                        strID = "-Shoulder|"
                    Case constShoulderServo
                        strID = "Shoulder |"
                    Case constElbowServo
                        strID = "Elbow    |"
                    Case constElbowRServo
                        strID = "-Elbow   |"
                    Case constWristRotateServo
                        strID = "Wrist R  |"
                    Case constGripperServo
                        strID = "Gripper  |"
                End Select
                If Not boolArmConnected Then
                    Select Case intServoID
                        Case constShoulderRServo
                            intPosition = ftnDegreeToPosition(taaArmAngles.sngShoulder)
                        Case constShoulderServo
                            intPosition = ftnDegreeToPosition(taaArmAngles.sngShoulder)
                        Case constElbowServo
                            intPosition = ftnDegreeToPosition(taaArmAngles.sngElbow - taaArmAngles.sngShoulder)
                        Case constElbowRServo
                            intPosition = ftnDegreeToPosition(taaArmAngles.sngElbow - taaArmAngles.sngShoulder)
                        Case constWristRotateServo
                            intPosition = ftnDegreeToPosition(taaArmAngles.sngWristRotate)
                        Case constGripperServo
                            intPosition = ftnDegreeToPosition(taaArmAngles.sngWrist)
                    End Select
                    intGoal = intPosition
                    intLoad = 0
                    intAngle = ftnPositionToDegree(intPosition)
                    intTemp = 22
                Else
                    intPosition = UnSafeNativeMethods.dxl_read_word(intServoID, constPresentPosition)
                    intGoal = UnSafeNativeMethods.dxl_read_word(intServoID, constGoalPosition)
                    intLoad = ftnGetDynaLoad(intServoID)
                    intAngle = Round(ftnPositionToDegree(intPosition))
                    intTemp = UnSafeNativeMethods.dxl_read_byte(intServoID, constTemperature)
                End If
                strText &= vbCrLf & strID & intGoal.ToString.PadLeft(6) & intPosition.ToString.PadLeft(7) & intAngle.ToString.PadLeft(6) _
                            & intLoad.ToString.PadLeft(6) & intTemp.ToString.PadLeft(6)
            Next intServoID
            rtbDynaParam.Text = strText
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In subDisplayDynaParameters :  " & ex.ToString)
        End Try
    End Sub


    ' =============================== BUTTONS =============================================================


    Delegate Sub DelegateHome(ByVal boolSetGripperToOpen As Boolean)
    Public Sub subHome(ByVal boolSetGripperToOpen As Boolean)
        Try
            If Me.InvokeRequired Then
                Dim newDelegate As New DelegateHome(AddressOf subHome)
                Me.Invoke(newDelegate, New Object() {boolSetGripperToOpen})
            Else
                boolLocationFocused = False
                If boolDebugArm Then frmDebug.subWriteLogs(" subHome: boolSetGripperToOpen = " & boolSetGripperToOpen)
                If Not boolStop And Not boolShutDown Then
                    taaArmAngles.sngShoulder = 110
                    taaArmAngles.sngElbow = 0
                    taaArmAngles.sngWrist = 0   '
                    taaArmAngles.sngWristRotate = 0
                    If boolSetGripperToOpen Then taaArmAngles.sngGripperOpening = ftnPositionToDegree(constGripperOpenPosition)
                    subMoveArmToNewAngles(taaArmAngles, True)
                End If
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In subHome :  " & ex.ToString)
        End Try
    End Sub

    Private Sub butReset_Click() Handles butReset.Click
        Try
            boolLocationFocused = False
            boolStop = False
            If Not boolArmConnected Or boolSimulatedMovement Then
                subHome(False)
            ElseIf Not boolStop And Not boolShutDown Then
                butStop_Click()
                For intServoID As Integer = 2 To 7
                    UnSafeNativeMethods.dxl_write_word(intServoID, constGoalPosition, UnSafeNativeMethods.dxl_read_word(intServoID, constPresentPosition))
                Next
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In butReset_Click :  " & ex.ToString)
        End Try
    End Sub

    Private Sub butHome_Click() Handles butHome.Click
        Try
            boolLocationFocused = False
            boolStop = False
            If boolDebugArm Then frmDebug.subWriteLogs(" ===============================  butHome_Click :  START")
            If Not boolStop And Not boolShutDown Then
                If NativeMethods.GetAsyncKeyState(Keys.LShiftKey) Then   ' 
                    subHome(False)  ' Don't open Gripper 
                Else
                    subHome(True)
                End If
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In butHome_Click :  " & ex.ToString)
        End Try
    End Sub

    Private Sub butStop_Click() Handles butStop.Click
        Try
            boolLocationFocused = False
            If boolArmConnected Then
                For intServoID As Integer = 2 To 7
                    UnSafeNativeMethods.dxl_write_word(intServoID, constGoalPosition, UnSafeNativeMethods.dxl_read_word(intServoID, constPresentPosition))
                    'UnSafeNativeMethods.dxl_write_word(intServoID, constTorqueLimit, UnSafeNativeMethods.dxl_read_word(intServoID, constTorqueMax))
                Next
                boolStop = True
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In butStop_Click :  " & ex.ToString)
        End Try
    End Sub





    Dim imageObject As Image
    Private Sub UpDnObject_ValueChanged() Handles UpDnObject.ValueChanged, textboxName.TextChanged
        Try
            boolLocationFocused = False
            Dim strObjectName As String = taoObjectArray(UpDnObject.Value).strName
            picObject.Visible = False
            If boolEnableRcc Then
                If (Dir("c:\Program Files\ER1\objects\" & strObjectName & ".bmp") <> "") Then
                    picObject.Visible = True
                    imageObject = System.Drawing.Image.FromFile("c:\Program Files\ER1\objects\" & strObjectName & ".bmp")
                ElseIf (Dir("c:\Program Files\ER1\objects\" & strObjectName & ".gif") <> "") Then
                    picObject.Visible = True
                    imageObject = System.Drawing.Image.FromFile("c:\Program Files\ER1\objects\" & strObjectName & ".gif")
                ElseIf (Dir("c:\Program Files\ER1\objects\" & strObjectName & ".jpeg") <> "") Then
                    picObject.Visible = True
                    imageObject = System.Drawing.Image.FromFile("c:\Program Files\ER1\objects\" & strObjectName & ".jpeg")
                ElseIf (Dir("c:\Program Files\ER1\objects\" & strObjectName & ".tiff") <> "") Then
                    picObject.Visible = True
                    imageObject = System.Drawing.Image.FromFile("c:\Program Files\ER1\objects\" & strObjectName & ".tiff")
                ElseIf (Dir("c:\Program Files\ER1\objects\" & strObjectName & ".png") <> "") Then
                    picObject.Visible = True
                    imageObject = System.Drawing.Image.FromFile("c:\Program Files\ER1\objects\" & strObjectName & ".png")
                ElseIf (Dir("c:\Program Files (x86)\ER1\objects\" & strObjectName & ".bmp") <> "") Then
                    picObject.Visible = True
                    imageObject = System.Drawing.Image.FromFile("c:\Program Files (x86)\ER1\objects\" & strObjectName & ".bmp")
                ElseIf (Dir("c:\Program Files (x86)\ER1\objects\" & strObjectName & ".gif") <> "") Then
                    picObject.Visible = True
                    imageObject = System.Drawing.Image.FromFile("c:\Program Files (x86)\ER1\objects\" & strObjectName & ".gif")
                ElseIf (Dir("c:\Program Files (x86)\ER1\objects\" & strObjectName & ".jpeg") <> "") Then
                    picObject.Visible = True
                    imageObject = System.Drawing.Image.FromFile("c:\Program Files (x86)\ER1\objects\" & strObjectName & ".jpeg")
                ElseIf (Dir("c:\Program Files (x86)\ER1\objects\" & strObjectName & ".tiff") <> "") Then
                    picObject.Visible = True
                    imageObject = System.Drawing.Image.FromFile("c:\Program Files (x86)\ER1\objects\" & strObjectName & ".tiff")
                ElseIf (Dir("c:\Program Files (x86)\ER1\objects\" & strObjectName & ".png") <> "") Then
                    picObject.Visible = True
                    imageObject = System.Drawing.Image.FromFile("c:\Program Files (x86)\ER1\objects\" & strObjectName & ".png")
                End If
            End If
            picObject.Image = imageObject
            frmLM_Image.picLM_Large.Image = imageObject
            If Not textboxName.Focused Then textboxName.Text = taoObjectArray(UpDnObject.Value).strName
            updnX.Value = taoObjectArray(UpDnObject.Value).tmcLocation.X
            updnY.Value = taoObjectArray(UpDnObject.Value).tmcLocation.Y
            updnHeight.Value = taoObjectArray(UpDnObject.Value).intObjectHeight - tjcJointCoordinates.tpcShoulderBase.Z
            updnOrientation.Value = taoObjectArray(UpDnObject.Value).intOrientation
            updnRotation.Value = taoObjectArray(UpDnObject.Value).intRotation
            upDnGripOffset.Value = taoObjectArray(UpDnObject.Value).intGripOffset_mm

        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In UpDnObject_ValueChanged :  " & ex.ToString)
        End Try
    End Sub


    Private Sub textboxName_TextChanged() Handles textboxName.TextChanged
        Try
            boolLocationFocused = False
            If textboxName.Focused Then
                boolSettingsChanged = True
                taoObjectArray(UpDnObject.Value).strName = textboxName.Text
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In textboxName_TextChanged :  " & ex.ToString)
        End Try
    End Sub

    Private Sub updnX_MouseClick() Handles updnX.MouseClick
        Try
            If updnX.Focused Then boolLocationFocused = True
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In updnX_MouseClick :  " & ex.ToString)
        End Try
    End Sub

    Private Sub updny_MouseClick(sender As Object, e As EventArgs) Handles updnY.MouseClick
        Try
            If updnY.Focused Then boolLocationFocused = True
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In updny_MouseClick :  " & ex.ToString)
        End Try
    End Sub

    Private Sub updnX_ValueChanged() Handles updnX.ValueChanged
        Try
            If updnX.Focused Or boolLocationFocused Then
                boolSettingsChanged = True
                subCheckMapArrayLimits(updnX.Value, updnY.Value)
                taoObjectArray(UpDnObject.Value).tmcLocation.X = updnX.Value
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In updnX_ValueChanged :  " & ex.ToString)
        End Try
    End Sub

    Private Sub updnY_ValueChanged() Handles updnY.ValueChanged
        Try
            If updnY.Focused Or boolLocationFocused Then
                boolSettingsChanged = True
                subCheckMapArrayLimits(updnX.Value, updnY.Value)
                taoObjectArray(UpDnObject.Value).tmcLocation.Y = updnY.Value
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In updnY_ValueChanged :  " & ex.ToString)
        End Try
    End Sub

    Delegate Sub DelegateUpdateObjectLocation(ByVal tmcLocation As typeMapCoordinate)
    Public Sub subUpdateObjectLocation(ByVal tmcLocation As typeMapCoordinate) '
        Try
            If Me.InvokeRequired Then
                Dim newDelegate As New DelegateUpdateObjectLocation(AddressOf subUpdateObjectLocation)
                Me.Invoke(newDelegate, New Object() {tmcLocation})
            Else
                updnX.Value = tmcLocation.X
                updnY.Value = tmcLocation.Y
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In subUpdateObjectLocation :  " & ex.ToString)
        End Try
    End Sub

    Private Sub updnHeight_ValueChanged() Handles updnHeight.ValueChanged
        Try
            If updnHeight.Focused Then
                boolLocationFocused = False
                boolSettingsChanged = True
                taoObjectArray(UpDnObject.Value).intObjectHeight = updnHeight.Value + tjcJointCoordinates.tpcShoulderBase.Z
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In updnY_ValueChanged :  " & ex.ToString)
        End Try
    End Sub

    Private Sub updnOrientation_ValueChanged() Handles updnOrientation.ValueChanged
        Try
            If updnOrientation.Focused Then
                boolLocationFocused = False
                boolSettingsChanged = True
                taoObjectArray(UpDnObject.Value).intOrientation = updnOrientation.Value
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In updnY_ValueChanged :  " & ex.ToString)
        End Try
    End Sub

    Private Sub updnRotation_ValueChanged() Handles updnRotation.ValueChanged
        Try
            If updnRotation.Focused Then
                boolLocationFocused = False
                boolSettingsChanged = True
                taoObjectArray(UpDnObject.Value).intRotation = updnRotation.Value
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In updnRotation_ValueChanged :  " & ex.ToString)
        End Try
    End Sub

    Private Sub upDnGripOffset_ValueChanged() Handles upDnGripOffset.ValueChanged
        Try
            If upDnGripOffset.Focused Then
                boolLocationFocused = False
                boolSettingsChanged = True
                taoObjectArray(UpDnObject.Value).intGripOffset_mm = upDnGripOffset.Value
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In upDnGripOffset_ValueChanged :  " & ex.ToString)
        End Try
    End Sub

    Private Sub butPickUp_Click() Handles butPickUp.Click
        Try
            boolLocationFocused = False
            boolStop = False
            If Not boolGetObjectBusy Then ftnGetObjectAuto(Me.UpDnObject.Value)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In butPickUp_Click :  " & ex.ToString)
        End Try
    End Sub


    Private Sub MoreDetails_Click() Handles butMoreDetails.Click
        Try
            Dim intServoID As Integer = updnServo.Value
            boolLocationFocused = False
            MsgBox("DYNAMIXEL # " & intServoID & ":  " & vbCrLf & _
        "Goal (R) = " & UnSafeNativeMethods.dxl_read_word(intServoID, constGoalPosition) & vbCrLf & _
        "Present Position = " & UnSafeNativeMethods.dxl_read_word(intServoID, constPresentPosition) & vbCrLf & _
        "Speed = " & UnSafeNativeMethods.dxl_read_word(intServoID, 32) & vbCrLf & _
        "CW Angle Limit = " & UnSafeNativeMethods.dxl_read_word(intServoID, 6) & vbCrLf & _
        "CCW Angle Limit = " & UnSafeNativeMethods.dxl_read_word(intServoID, 8) & vbCrLf & _
        "CW Compliance = " & UnSafeNativeMethods.dxl_read_byte(intServoID, 26) & vbCrLf & _
        "CCW Compliance = " & UnSafeNativeMethods.dxl_read_byte(intServoID, 27) & vbCrLf & _
        "Torque Limit = " & UnSafeNativeMethods.dxl_read_word(intServoID, constTorqueLimit) & vbCrLf & _
        "Torque Enable = " & UnSafeNativeMethods.dxl_read_byte(intServoID, 24) & vbCrLf & _
        "Timeout = " & (UnSafeNativeMethods.dxl_get_result() = COMM_RXTIMEOUT) & vbCrLf & _
        "High limit T = " & UnSafeNativeMethods.dxl_read_byte(intServoID, 11) & vbCrLf & _
        "Temperature = " & UnSafeNativeMethods.dxl_read_byte(intServoID, 43) & vbCrLf & _
        "Low limit V = " & UnSafeNativeMethods.dxl_read_byte(intServoID, 12) * 0.1 & "V" & vbCrLf & _
        "High limit V = " & UnSafeNativeMethods.dxl_read_byte(intServoID, 13) * 0.1 & "V" & vbCrLf & _
        "Voltage = " & UnSafeNativeMethods.dxl_read_byte(intServoID, 42) * 0.1 & "V" & vbCrLf & _
        "Alarm Shutdown = " & UnSafeNativeMethods.dxl_read_byte(intServoID, constAlarmShutDown))
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In subDisplayDynamixelValues :  " & ex.ToString)
        End Try
    End Sub

    Private Sub updnComPort_ValueChanged() Handles updnComPort.ValueChanged
        Try
            boolLocationFocused = False
            If updnComPort.Focused Then
                boolSettingsChanged = True
                intArmComPort = updnComPort.Value
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In updnComPort_ValueChanged :  " & ex.ToString)
        End Try
    End Sub


    Private Sub tsslArmStatus_Click(sender As Object, e As EventArgs) Handles tsslArmStatus.Click
        Try
            boolLocationFocused = False
            ftnArmLoad()
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In tsslArmStatus_Click :  " & ex.ToString)
        End Try
    End Sub
End Class
