
Imports System.Math


Partial Public Class frmArm


    ' NOTE - dimensions are mmm and degrees

    'http://www.cerebralmeltdown.com/Robot%20Arm/Robot%20Arm%20Math/index.html

    Public Function ftnGetArmAngles() As typeArmAngles
        ' Shoulder and elbow angles are relative to ground
        Try
            If Not boolArmConnected Or boolSimulatedMovement Then
                ftnGetArmAngles = taaArmAngles
            Else
                ftnGetArmAngles.sngShoulder = (ftnPositionToDegree(UnSafeNativeMethods.dxl_read_word(constShoulderServo, constPresentPosition)) - ftnPositionToDegree(UnSafeNativeMethods.dxl_read_word(constShoulderRServo, constPresentPosition))) / 2
                ftnGetArmAngles.sngElbow = ftnConvertTo180(ftnGetArmAngles.sngShoulder + (ftnPositionToDegree(UnSafeNativeMethods.dxl_read_word(constElbowServo, constPresentPosition)) - ftnPositionToDegree(UnSafeNativeMethods.dxl_read_word(constElbowRServo, constPresentPosition))) / 2)
                ftnGetArmAngles.sngWristRotate = ftnPositionToDegree(UnSafeNativeMethods.dxl_read_word(constWristRotateServo, constPresentPosition))
                ftnGetArmAngles.sngWrist = ftnPositionToDegree(UnSafeNativeMethods.dxl_read_word(constWristRotateServo, constPresentPosition))
                ftnGetArmAngles.sngGripperOpening = ftnPositionToDegree(UnSafeNativeMethods.dxl_read_word(constGripperServo, constPresentPosition))
                ftnGetArmAngles = ftnArmAnglesCorrectedForDeflection(ftnGetArmAngles)
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnGetArmAngles :  " & ex.ToString)
        End Try
    End Function

    Public Function ftnForwardKinematicsCoordinates(ByVal taaArmAnglesLocal As typeArmAngles) As typeJointCoordinate
        ' Arm dimensions in mm
        Try
            ftnForwardKinematicsCoordinates.tpcShoulderBase.X = 0
            ftnForwardKinematicsCoordinates.tpcShoulderBase.Y = constShoulderBaseY
            ftnForwardKinematicsCoordinates.tpcShoulderBase.Z = constShoulderBaseZ
            ftnForwardKinematicsCoordinates.tpcShoulder = ftnPointFromPoint(ftnForwardKinematicsCoordinates.tpcShoulderBase, 0, constBaseAngleYZ, 0)
            ftnForwardKinematicsCoordinates.tpcElbow = ftnPointFromPoint(ftnForwardKinematicsCoordinates.tpcShoulder, constShoulderToElbow, taaArmAnglesLocal.sngShoulder, 0)
            ftnForwardKinematicsCoordinates.tpcWristRotateB4Offset = ftnPointFromPoint(ftnForwardKinematicsCoordinates.tpcElbow, constElbowToWristR, taaArmAnglesLocal.sngElbow, 0)
            ftnForwardKinematicsCoordinates.tpcWristRotate = ftnPointFromPoint(ftnForwardKinematicsCoordinates.tpcWristRotateB4Offset, constWristROffset, ftnConvertTo180(taaArmAnglesLocal.sngElbow - 90), 0)
            ftnForwardKinematicsCoordinates.tpcWrist = ftnPointFromPoint(ftnForwardKinematicsCoordinates.tpcWristRotate, constWristRToWrist, taaArmAnglesLocal.sngElbow, 0)
            ftnForwardKinematicsCoordinates.tpcCamera.X = 0
            ftnForwardKinematicsCoordinates.tpcCamera.Y = ftnForwardKinematicsCoordinates.tpcWrist.Y + constCameraOffsetY * Cos(ftnDegreeToRadian(taaArmAnglesLocal.sngElbow)) _
                                                        - constCameraOffsetZ * Sin(ftnDegreeToRadian(taaArmAnglesLocal.sngElbow))
            ftnForwardKinematicsCoordinates.tpcCamera.Z = ftnForwardKinematicsCoordinates.tpcWrist.Z + constCameraOffsetZ * Abs(Cos(ftnDegreeToRadian(taaArmAnglesLocal.sngElbow))) _
                                                        + constCameraOffsetY * Sin(ftnDegreeToRadian(taaArmAnglesLocal.sngElbow))
            ftnForwardKinematicsCoordinates.tpcGripperEnd = ftnPointFromPoint(ftnForwardKinematicsCoordinates.tpcWrist, constWristToGripper, taaArmAnglesLocal.sngElbow, 0)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnForwardKinematicsCoordinates :  " & ex.ToString)
        End Try
    End Function

    '===============================================  Check For Valid Arm Angles  ================================

    Function ftnValidKinematics(ByVal taaArmAnglesLocal As typeArmAngles) As Boolean
        Try
            ftnValidKinematics = True
            Dim sngElbowJointAngle As Single = ftnConvertTo180(taaArmAnglesLocal.sngElbow - taaArmAnglesLocal.sngShoulder)
            Dim tjcJointCoordinates As typeJointCoordinate = ftnForwardKinematicsCoordinates(taaArmAnglesLocal)
            If (taaArmAnglesLocal.sngShoulder > (constMaxJointAngle + 5)) Or (taaArmAnglesLocal.sngShoulder < -75) Then
                ftnValidKinematics = False
            ElseIf (sngElbowJointAngle > (constMaxJointAngle + 5)) Or (sngElbowJointAngle < -(constMaxJointAngle + 5)) Then
                ftnValidKinematics = False
            ElseIf (tjcJointCoordinates.tpcElbow.Y < 0) And (tjcJointCoordinates.tpcElbow.Z < 100) Then
                ftnValidKinematics = False
            ElseIf (tjcJointCoordinates.tpcGripperEnd.Y < 0) And (tjcJointCoordinates.tpcGripperEnd.Z < 300) Then
                ftnValidKinematics = False
            ElseIf (tjcJointCoordinates.tpcGripperEnd.Z < -330) Then
                ftnValidKinematics = False
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnValidKinematics :  " & ex.ToString)
        End Try
    End Function













End Class
