

Imports System.Math
Imports System.Drawing


Partial Public Class frmArm



    Public Function ftnSetShoulderAngle(ByVal sngAngle As Single) As Boolean
        Try
            sngAngle = ftnJointAngleVerify(sngAngle)
            Dim intPosition As Short = ftnDegreeToPosition(sngAngle)
            ftnSetShoulderAngle = ftnSetDynamixelPosition(constShoulderRServo, 1024 - intPosition) And ftnSetDynamixelPosition(constShoulderServo, intPosition)
            If boolSimulatedMovement Then taaArmAngles.sngShoulder = sngAngle
            If boolDebugArm Then frmDebug.subWriteLogs(" ftnSetShoulderPosition: sngAngle = " & sngAngle & " /  intPosition = " & intPosition)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In ftnSetShoulderAngle :  " & ex.ToString)
        End Try
    End Function

    Public Function ftnSetElbowAngle(ByVal sngAngle As Single) As Boolean
        Try
            sngAngle = ftnJointAngleVerify(sngAngle)
            Dim intPosition As Short = ftnDegreeToPosition(sngAngle)
            ftnSetElbowAngle = ftnSetDynamixelPosition(constElbowServo, intPosition) And ftnSetDynamixelPosition(constElbowRServo, 1024 - intPosition)
            If boolSimulatedMovement Then taaArmAngles.sngElbow = sngAngle + taaArmAngles.sngShoulder
            If boolDebugArm Then frmDebug.subWriteLogs(" ftnSetElbowPosition: sngAngle = " & sngAngle & " /  intPosition = " & intPosition)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In ftnSetElbowAngle :  " & ex.ToString)
            ftnSetElbowAngle = False
        End Try
    End Function


    Public Function ftnSetWristRotateAngle(ByVal sngAngle As Single) As Boolean
        Try
            sngAngle = ftnJointAngleVerify(sngAngle)
            taaArmAngles.sngWristRotate = Max(sngAngle, -constMaxJointAngle)
            taaArmAngles.sngWristRotate = Min(taaArmAngles.sngWristRotate, constMaxJointAngle)
            Dim intPosition As Short = ftnDegreeToPosition(sngAngle)
            ftnSetWristRotateAngle = ftnSetDynamixelPosition(constWristRotateServo, intPosition)
            If boolSimulatedMovement Then taaArmAngles.sngWristRotate = sngAngle
            If boolDebugArm Then frmDebug.subWriteLogs(" ftnSetWristRotateAngle: sngAngle = " & sngAngle & " /  intPosition = " & intPosition)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In ftnSetWristRotateAngle :  " & ex.ToString)
        End Try
    End Function

    Public Function ftnSetGripperPosition(ByVal intPosition As Short) As Boolean
        Try
            If Not boolStop And Not boolShutDown Then
                intPosition = Min(Max(intPosition, constGripperOpenPosition), constGripperClosedPosition)
                taaArmAngles.sngGripperOpening = ftnPositionToDegree(intPosition)
                ftnSetGripperPosition = ftnSetDynamixelPosition(constGripperServo, intPosition)
                If boolDebugArm Then frmDebug.subWriteLogs(" ftnSetGripperPosition: sngAngle = " & taaArmAngles.sngGripperOpening & " /  intPosition = " & intPosition)
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In ftnSetGripperPosition :  " & ex.ToString)
        End Try
    End Function

    Public Function ftnGripperDegreesToWidth(ByRef sngDegrees As Single) As Single ' in mm
        Try
            Dim intClosedDegrees = ftnPositionToDegree(constGripperClosedPosition)
            Dim intOpenDegrees = ftnPositionToDegree(constGripperOpenPosition)
            ftnGripperDegreesToWidth = (intClosedDegrees - sngDegrees) * 55 / (intClosedDegrees - intOpenDegrees) ' 
            ftnGripperDegreesToWidth = Min(Max(ftnGripperDegreesToWidth, 0), 55)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In ftnGripperDegreesToWidth :  " & ex.ToString)
        End Try
    End Function

    Public Function ftnSetDynamixelPosition(ByVal intServoID As Short, ByRef intPosition As Short) As Boolean
        Try
            ftnSetDynamixelPosition = True
            If boolDebugArm Then frmDebug.subWriteLogs(" ftnSetDynamixelPosition: intServoID = " & intServoID & ", intPosition = " & intPosition & ": " & boolArmConnected)
            If boolSimulatedMovement Then
                subWait(50)
            ElseIf Not boolStop And Not boolShutDown Then
                If (intPosition > 1023) Then intPosition = 1023
                If (intPosition < 0) Then intPosition = 0
                If boolDrivingToDestination Then UnSafeNativeMethods.dxl_write_word(intServoID, constTorqueLimit, 1023) '  
                UnSafeNativeMethods.dxl_write_word(intServoID, constGoalPosition, intPosition)
                ftnSetDynamixelPosition = (UnSafeNativeMethods.dxl_get_result() = COMM_RXSUCCESS) And Not (UnSafeNativeMethods.dxl_get_rxpacket_error(ERRBIT_OVERLOAD) = 1) And Not (UnSafeNativeMethods.dxl_get_rxpacket_error(ERRBIT_OVERHEAT) = 1)
                timeJointMove(intServoID) = Now()
                If Not ftnSetDynamixelPosition Then
                    frmDebug.subWriteLogs(" ftnSetDynamixelPosition Fail : dxl_get_result() =  " _
                                          & UnSafeNativeMethods.dxl_get_result() & "  / dynamixel.dxl_get_rxpacket_error(dynamixel.ERRBIT_OVERLOAD) = " & UnSafeNativeMethods.dxl_get_rxpacket_error(ERRBIT_OVERLOAD) _
                                          & " dynamixel.dxl_get_rxpacket_error(dynamixel.ERRBIT_OVERHEAT) = " & UnSafeNativeMethods.dxl_get_rxpacket_error(ERRBIT_OVERHEAT))
                    subHighlightDynamixelError(intServoID)
                End If
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In ftnSetDynamixelPosition :  " & ex.ToString)
        End Try
    End Function


    Public Sub subHighlightDynamixelError(ByVal intServo As Integer)
        Try
            Select Case intServo
                Case constShoulderRServo
                    subDrawArmVector(tjcJointCoordinates.tpcShoulder, tjcJointCoordinates.tpcElbow, constShoulderSegment, True)
                Case constShoulderServo
                    subDrawArmVector(tjcJointCoordinates.tpcShoulder, tjcJointCoordinates.tpcElbow, constShoulderSegment, True)
                Case constElbowServo
                    subDrawArmVector(tjcJointCoordinates.tpcElbow, tjcJointCoordinates.tpcWristRotate, constElbowSegment, True)
                Case constElbowRServo
                    subDrawArmVector(tjcJointCoordinates.tpcElbow, tjcJointCoordinates.tpcWristRotate, constElbowSegment, True)
                Case constWristRotateServo
                    subDrawArmVector(tjcJointCoordinates.tpcWristRotate, tjcJointCoordinates.tpcWrist, constWristRotateSegment, True)
                Case constGripperServo
                    subDrawArmVector(tjcJointCoordinates.tpcWrist, tjcJointCoordinates.tpcGripperEnd, constGripperSegment, True)
                Case 8
                    subDrawArmVector(tjcJointCoordinates.tpcShoulderBase, tjcJointCoordinates.tpcShoulder, constShoulderSegment, True)
                Case 9
                    subDrawArmVector(tjcJointCoordinates.tpcWrist, tjcJointCoordinates.tpcGripperEnd, constWristSegment, True)
                Case Else
            End Select
            frmDebug.subWriteLogs(" subHighlightDynamixelError:  intServo =  " & intServo)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION In subHighlightDynamixelError :  " & ex.ToString)
        End Try
    End Sub




End Class
