from ctypes import *
import time,  platform

def enum(**enums):
    return type("Enum", (), enums)

EndType = enum(EndTypeCustom=0, 
    EndTypeSuctionCup=1, 
    EndTypeGripper=2, 
    EndTypeLaser=3,
    EndTypePen = 4,  
    EndTypeMax=5)

# For EndTypeParams
class EndTypeParams(Structure):
    _pack_ = 1
    _fields_ = [
        ("xBias", c_float),
        ("yBias", c_float),
        ("zBias", c_float)
        ]

class Pose(Structure):
    _pack_ = 1
    _fields_ = [
        ("x", c_float),
        ("y", c_float),
        ("z", c_float),
        ("rHead", c_float),
        ("joint1Angle", c_float),
        ("joint2Angle", c_float),
        ("joint3Angle", c_float),
        ("joint4Angle", c_float)
        ]

class Kinematics(Structure):
    _pack_ = 1
    _fields_ = [
        ("velocity", c_float),
        ("acceleration", c_float)
        ]

class AlarmsState(Structure):
    _pack_ = 1
    _fields_ = [
        ("alarmsState", c_int32)
        ]

class HOMEParams(Structure):
    _pack_ = 1
    _fields_ = [
        ("x", c_float), 
        ("y", c_float), 
        ("z", c_float), 
        ("r", c_float)
        ]

class HOMECmd(Structure):
    _pack_ = 1
    _fields_ = [
        ("temp", c_float)
        ]
        
class AutoLevelingCmd(Structure):
    _pack_ = 1
    _fields_ = [
        ("controlFlag", c_ubyte),
        ("precision", c_float)
        ]
        
class EMotor(Structure):
    _pack_ = 1
    _fields_ = [
        ("index", c_byte), 
        ("isEnabled", c_byte), 
        ("speed", c_int32)
        ]
        
class EMotorS(Structure):
    _pack_ = 1
    _fields_ = [
        ("index", c_byte), 
        ("isEnabled", c_byte), 
        ("speed", c_int32), 
        ("distance", c_uint32)
        ]
        
##################  Arm orientation定义   ##################
ArmOrientation = enum(
    LeftyArmOrientation=0, 
    RightyArmOrientation=1)
    
##################  点动示教部分   ##################

class JOGJointParams(Structure):
    _pack_ = 1
    _fields_ = [
        ("joint1Velocity", c_float), 
        ("joint2Velocity", c_float), 
        ("joint3Velocity", c_float), 
        ("joint4Velocity", c_float), 
        ("joint1Acceleration", c_float),
        ("joint2Acceleration", c_float),
        ("joint3Acceleration", c_float),
        ("joint4Acceleration", c_float)
        ]

class JOGCoordinateParams(Structure):
    _pack_ = 1
    _fields_ = [
        ("xVelocity", c_float), 
        ("yVelocity", c_float), 
        ("zVelocity", c_float), 
        ("rVelocity", c_float), 
        ("xAcceleration", c_float),
        ("yAcceleration", c_float),
        ("zAcceleration", c_float),
        ("rAcceleration", c_float)
        ]

class JOGCommonParams(Structure):
    _pack_ = 1
    _fields_ = [
        ("velocityRatio", c_float), 
        ("accelerationRatio", c_float)
        ]

class JOGLParams(Structure):
    _pack_ = 1
    _fields_ = [
        ("velocity",  c_float), 
        ("acceleration",  c_float)
    ]


JC = enum(JogIdle=0, 
    JogAPPressed=1, 
    JogANPressed=2, 
    JogBPPressed=3, 
    JogBNPressed=4,
    JogCPPressed=5,
    JogCNPressed=6,
    JogDPPressed=7,
    JogDNPressed=8,
    JogEPPressed=9,
    JogENPressed=10)

class JOGCmd(Structure):
    _pack_ = 1
    _fields_ = [
        ("isJoint", c_byte), 
        ("cmd", c_byte)
        ]

##################  再现运动部分   ##################

class PTPJointParams(Structure):
    _fields_ = [
        ("joint1Velocity", c_float), 
        ("joint2Velocity", c_float), 
        ("joint3Velocity", c_float), 
        ("joint4Velocity", c_float), 
        ("joint1Acceleration", c_float),
        ("joint2Acceleration", c_float),
        ("joint3Acceleration", c_float),
        ("joint4Acceleration", c_float)
        ]
        
class PTPCoordinateParams(Structure):
    _fields_ = [
        ("xyzVelocity", c_float), 
        ("rVelocity", c_float),
        ("xyzAcceleration", c_float), 
        ("rAcceleration", c_float)
        ]

class PTPLParams(Structure):
    _pack_ = 1
    _fields_ = [
        ("velocity",  c_float), 
        ("acceleration",  c_float)
    ]

class PTPJumpParams(Structure):
    _pack_ = 1
    _fields_ = [
        ("jumpHeight", c_float), 
        ("zLimit", c_float)
        ]

class PTPCommonParams(Structure):
    _pack_ = 1
    _fields_ = [
        ("velocityRatio", c_float), 
        ("accelerationRatio", c_float)
        ]

PTPMode = enum(
    PTPJUMPXYZMode=0,
    PTPMOVJXYZMode=1,
    PTPMOVLXYZMode=2,
    
    PTPJUMPANGLEMode=3,
    PTPMOVJANGLEMode=4,
    PTPMOVLANGLEMode=5,
    
    PTPMOVJANGLEINCMode=6,
    PTPMOVLXYZINCMode=7, 
    PTPMOVJXYZINCMode=8, 
    
    PTPJUMPMOVLXYZMode=9)

InputPin = enum( InputPinNone=0,
    InputPin1=1,
    InputPin2=2,
    InputPin3=3,
    InputPin4=4,
    InputPin5=5,
    InputPin6=6,
    InputPin7=7,
    InputPin8=8)

InputLevel = enum(InputLevelBoth=0,
    InputLevelLow=1,
    InputLevelHigh=2)

OutputPin = enum(
    SIGNALS_O1=1,
    SIGNALS_O2=2,
    SIGNALS_O3=3,
    SIGNALS_O4=4,
    SIGNALS_O5=5,
    SIGNALS_O6=6,
    SIGNALS_O7=7,
    SIGNALS_O8=8)

class PTPCmd(Structure):
    _pack_ = 1
    _fields_ = [
        ("ptpMode", c_byte),
        ("x", c_float),
        ("y", c_float),
        ("z", c_float),
        ("rHead", c_float)
        ]
        
class DeviceCountInfo(Structure):
    _pack_ = 1
    _fields_ = [
        ("deviceRunTime",  c_uint64),
        ("devicePowerOn",  c_uint32),
        ("devicePowerOff", c_uint32)
        ]
        
class PTPWithLCmd(Structure):
    _pack_ = 1
    _fields_ = [
        ("ptpMode", c_byte),
        ("x", c_float),
        ("y", c_float),
        ("z", c_float),
        ("rHead", c_float),
        ("l", c_float)
        ]

##################  Continuous path   ##################

class CPParams(Structure):
    _pack_ = 1
    _fields_ = [
        ("planAcc", c_float),
        ("juncitionVel", c_float),
        ("acc", c_float), 
        ("realTimeTrack",  c_byte)
        ]

ContinuousPathMode = enum(
    CPRelativeMode=0,
    CPAbsoluteMode=1)

class CPCmd(Structure):
    _pack_ = 1
    _fields_ = [
        ("cpMode", c_byte),
        ("x", c_float),
        ("y", c_float),
        ("z", c_float),
        ("velocity", c_float)
        ]
        
class CPCommonParams(Structure):
    _pack_ = 1
    _fields_ = [
        ("velocityRatio", c_float), 
        ("accelerationRatio", c_float)
        ]

##################  圆弧：ARC   ##################
class ARCPoint(Structure):
    _pack_ = 1
    _fields_ = [
        ("x", c_float),
        ("y", c_float),
        ("z", c_float),
        ("rHead", c_float)
    ]
        
class ARCParams(Structure):
    _pack_ = 1
    _fields_ = [
        ("xyzVelocity", c_float), 
        ("rVelocity", c_float),
        ("xyzAcceleration", c_float), 
        ("rAcceleration", c_float)
        ]

class ARCCmd(Structure):
    _pack_ = 1
    _fields_ = [
        ("cirPoint", ARCPoint),
        ("toPoint", ARCPoint)
    ]
    
class CircleCmd(Structure):
    _pack_ = 1
    _fields_ = [
        ("cirPoint", ARCPoint),
        ("toPoint", ARCPoint)
    ]

class ARCCommonParams(Structure):
    _pack_ = 1
    _fields_ = [
        ("velocityRatio", c_float), 
        ("accelerationRatio", c_float)
        ]

##################  User parameters   ##################

class WAITParams(Structure):
    _pack_ = 1
    _fields_ = [
        ("unitType", c_byte)
        ]

class WAITCmd(Structure):
    _pack_ = 1
    _fields_ = [
        ("waitTime", c_uint32)
        ]

TRIGMode = enum(
    TRIGInputIOMode = 0,
    TRIGADCMode=1)
    
TRIGInputIOCondition = enum(
    TRIGInputIOEqual = 0,
    TRIGInputIONotEqual=1)
    
TRIGADCCondition = enum(
    TRIGADCLT = 0,
    TRIGADCLE=1, 
    TRIGADCGE = 2,
    TRIGADCGT=3)
    
class TRIGCmd(Structure):
    _pack_ = 1
    _fields_ = [
        ("address", c_byte), 
        ("mode", c_byte), 
        ("condition",  c_byte), 
        ("threshold", c_uint16)
        ]

GPIOType = enum(
    GPIOTypeDummy = 0, 
    GPIOTypeDO = 1,
    GPIOTypePWM=2,
    GPIOTypeDI=3, 
    GPIOTypeADC=4, 
    GPIOTypeDIPU=5, 
    GPIOTypeDIPD=6)
    
class IOMultiplexing(Structure):
    _pack_ = 1
    _fields_ = [
        ("address", c_byte), 
        ("multiplex", c_byte)
        ]
        
class IODO(Structure):
    _pack_ = 1
    _fields_ = [
        ("address", c_byte), 
        ("level", c_byte)
        ]
        
class IOPWM(Structure):
    _pack_ = 1
    _fields_ = [
        ("address", c_byte), 
        ("frequency", c_float), 
        ("dutyCycle", c_float)
        ]
        
class IODI(Structure):
    _pack_ = 1
    _fields_ = [
        ("address", c_byte), 
        ("level", c_byte)
        ]
        
class IOADC(Structure):
    _pack_ = 1
    _fields_ = [
        ("address", c_byte), 
        ("value", c_int)
        ]

class UserParams(Structure):
    _pack_ = 1
    _fields_ = [
        ("params1", c_float),
        ("params2", c_float),
        ("params3", c_float),
        ("params4", c_float),
        ("params5", c_float),
        ("params6", c_float),
        ("params7", c_float),
        ("params8", c_float)
        ]

ZDFCalibStatus = enum(
    ZDFCalibNotFinished=0,
    ZDFCalibFinished=1)
    
"""
WIFI
JoMar 
20160906
"""
class WIFIIPAddress(Structure):
    _pack_ = 1
    _fields_ = [
        ("dhcp", c_byte),
        ("addr1", c_byte),
        ("addr2", c_byte),
        ("addr3", c_byte),
        ("addr4", c_byte),
        ]
        
class WIFINetmask(Structure):
    _pack_ = 1
    _fields_ = [
        ("addr1", c_byte),
        ("addr2", c_byte),
        ("addr3", c_byte),
        ("addr4", c_byte),
        ]
        
class WIFIGateway(Structure):
    _pack_ = 1
    _fields_ = [
        ("addr1", c_byte),
        ("addr2", c_byte),
        ("addr3", c_byte),
        ("addr4", c_byte),
        ]
        
class WIFIDNS(Structure):
    _pack_ = 1
    _fields_ = [
        ("addr1", c_byte),
        ("addr2", c_byte),
        ("addr3", c_byte),
        ("addr4", c_byte),
        ]

ColorPort = enum(
    PORT_GP1 = 0, 
    PORT_GP2 = 1,
    PORT_GP4 = 2,
    PORT_GP5 = 3
    )
    
InfraredPort = enum(
    PORT_GP1 = 0, 
    PORT_GP2 = 1,
    PORT_GP4 = 2,
    PORT_GP5 = 3
    )
    
UART4PeripheralsType = enum(
    UART4PeripheralsUART = 0,
    UART4PeripheralsWIFI = 1,
    UART4PeripheralsBLE = 2,
    UART4PeripheralsCH375 = 3
    )
##################  API result   ##################

DobotConnect = enum(
    DobotConnect_NoError=0,
    DobotConnect_NotFound=1,
    DobotConnect_Occupied=2)

DobotCommunicate = enum(
    DobotCommunicate_NoError=0,
    DobotCommunicate_BufferFull=1,
    DobotCommunicate_Timeout=2)

##################  API func   ##################

#parker add 2018 8 29 添加Wifi设置模块退出标志位
QuitDobotApiFlag = True

def load(isUseForSearch=False):
    if platform.system() == "Windows":
        if isUseForSearch:
            return CDLL("DobotDll-SerialName.dll",  RTLD_GLOBAL)
        else:
            return CDLL("DobotDll.dll",  RTLD_GLOBAL)
    elif platform.system() == "Darwin" :
        return CDLL("libDobotDll.dylib",  RTLD_GLOBAL)
    elif platform.system() == "Linux":
        return cdll.loadLibrary("libDobotDll.so")

#
def dSleep(ms):
    # t = time.time()
    # s = ms / 1000.0
    # while(time.time() - t < s):
    #     QCoreApplication.processEvents(QEventLoop.AllEvents, 5)

    time.sleep(ms / 1000)
    #parker edit 2018 8 29 测试发现这种方法比上面的更节省CPU
    # QThread.msleep(ms)
    # qApp.processEvents(QEventLoop.AllEvents)


def gettime():
    return [time.time()]
    
#
def SearchDobot(api,  maxLen=1000):
    szPara = create_string_buffer(1000) #((len(str(maxLen)) + 4) * maxLen + 10)
    l = api.SearchDobot(szPara,  maxLen)
    if l == 0:
        return []
    ret = szPara.value.decode("utf-8") 
    
    def fix(devices):
        for index in range(len(devices)):
            device = devices[index]
            if "(" in device and ")" not in device:
                yield device + " " + devices[index + 1]
            elif  "(" not in device and ")" in device:
                pass
            else:
                yield device
        
    return list(fix(ret.split(" ")))
    

def ConnectDobot(api, portName,  baudrate):
    szPara = create_string_buffer(100)
    szPara.raw = portName.encode("utf-8") 
    fwType = create_string_buffer(100)
    version = create_string_buffer(100)
    time = c_float(0)
    result = api.ConnectDobot(szPara,  baudrate,  fwType,  version, byref(time))
    return [result,  fwType.value.decode("utf-8"),  version.value.decode("utf-8"), time.value]


def DisconnectDobot(api):
    api.DisconnectDobot()


def GetMarlinVersion(api):
    api.GetMarlinVersion()


def PeriodicTask(api):
    api.PeriodicTask()


def SetCmdTimeout(api, times):
    api.SetCmdTimeout(times)


def DobotExec(api):
    return [api.DobotExec()]

#
def GetQueuedCmdCurrentIndex(api):
    queuedCmdIndex = c_uint64(0)
    while(True):
        result = api.GetQueuedCmdCurrentIndex(byref(queuedCmdIndex))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(2)
            continue
        break
    return [queuedCmdIndex.value]


def GetQueuedCmdMotionFinish(api):
    isFinish = c_bool(False)
    while(True):
        result = api.GetQueuedCmdMotionFinish(byref(isFinish))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(2)
            continue
        break
    return [isFinish.value]


def SetQueuedCmdStartExec(api):
    while(True):
        result = api.SetQueuedCmdStartExec()
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break


def SetQueuedCmdStopExec(api):
    while(True):
        result = api.SetQueuedCmdStopExec()
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
       
 
def SetQueuedCmdForceStopExec(api):
    while(True):
        result = api.SetQueuedCmdForceStopExec()
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    

def SetQueuedCmdStartDownload(api,  totalLoop, linePerLoop):
    while(True):
        result = api.SetQueuedCmdStartDownload(totalLoop, linePerLoop)
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
        

def SetQueuedCmdStopDownload(api):
    while(True):
        result = api.SetQueuedCmdStopDownload()
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    

def SetQueuedCmdClear(api):
    return [api.SetQueuedCmdClear()]


def SetDeviceSN(api, str): 
    szPara = create_string_buffer(25)
    szPara.raw = str.encode("utf-8")
    while(True):
        result = api.SetDeviceSN(szPara)
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break


def GetDeviceSN(api): 
    szPara = create_string_buffer(25)
    while(True):
        result = api.GetDeviceSN(szPara,  25)
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    ret = szPara.value.decode("utf-8") 
    return [ret]


def SetDeviceName(api, str): 
    szPara = create_string_buffer(len(str) * 4)
    szPara.raw = str.encode("utf-8")
    while(True):
        result = api.SetDeviceName(szPara)
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
        

def SetDeviceNumName(api, num): 
    cNum = c_int(num)
    while(True):
        result = api.SetDeviceName(cNum)
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break


def GetDeviceName(api): 
    szPara = create_string_buffer(66)  
    while(True):
        result = api.GetDeviceName(szPara,  100)
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    ret = szPara.value.decode("utf-8")
    return [ret]
    

def GetDeviceVersion(api):
    majorVersion = c_byte(0)
    minorVersion = c_byte(0)
    revision     = c_byte(0)
    hwVersion    = c_byte(0)
    while(True):
        result = api.GetDeviceVersion(byref(majorVersion), byref(minorVersion), byref(revision), byref(hwVersion))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [majorVersion.value, minorVersion.value, revision.value, hwVersion.value]


def SetDeviceWithL(api, isWithL, version=0, isQueued=0):
    queuedCmdIndex = c_uint64(0)
    while(True):
        result = api.SetDeviceWithL(c_bool(isWithL), c_uint8(version), c_bool(isQueued), byref(queuedCmdIndex))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [queuedCmdIndex.value]


def GetDeviceWithL(api):
    isWithL = c_bool(False)
    while(True):
        result = api.GetDeviceWithL(byref(isWithL))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [isWithL.value]


def GetDeviceTime(api):
    time = c_uint32(0)
    while(True):
        result = api.GetDeviceTime(byref(time))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [time.value]


def GetDeviceInfo(api):
    info = DeviceCountInfo()
    while(True):
        result = api.GetDeviceInfo(byref(info))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [info.deviceRunTime, info.devicePowerOn, info.devicePowerOff]


def ResetPose(api, manual, rearArmAngle, frontArmAngle):
    c_rearArmAngle = c_float(rearArmAngle)
    c_frontArmAngle = c_float(frontArmAngle)
    while(True):
        result = api.ResetPose(manual, c_rearArmAngle, c_frontArmAngle)
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break

#
def GetPose(api):
    pose = Pose()
    while(True):
        result = api.GetPose(byref(pose))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [pose.x, pose.y, pose.z,pose.rHead, pose.joint1Angle, pose.joint2Angle, pose.joint3Angle, pose.joint4Angle]

#
def GetPoseL(api):
    l = c_float(0)
    while(True):
        result = api.GetPoseL(byref(l))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [l.value]


def GetKinematics(api):
    kinematics = Kinematics()
    while(True):
        result = api.GetKinematics(byref(kinematics))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [kinematics.velocity, kinematics.acceleration]


def GetAlarmsState(api,  maxLen=1000):
    alarmsState = create_string_buffer(maxLen) 
    #alarmsState = c_byte(0)
    len = c_int(0)
    while(True):
        result = api.GetAlarmsState(alarmsState,  byref(len),  maxLen)
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [alarmsState.raw, len.value]
    
#
def ClearAllAlarmsState(api):
    while(True):
        result = api.ClearAllAlarmsState()
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break


def GetUserParams(api):
    param = UserParams()
    while(True):
        result = api.GetUserParams(byref(param))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [param.params1,param.params2,param.params3,param.params4,param.params5,param.params6,param.params7,param.params8]


def SetHOMEParams(api,  x,  y,  z,  r,  isQueued=0):
    param = HOMEParams()
    param.x = x
    param.y = y
    param.z = z
    param.r = r
    queuedCmdIndex = c_uint64(0)
    while(True):
        result = api.SetHOMEParams(byref(param),  isQueued, byref(queuedCmdIndex))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [queuedCmdIndex.value]


def GetHOMEParams(api):
    param = HOMEParams()
    while(True):
        result = api.GetHOMEParams(byref(param))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [param.x, param.y, param.z, param.r]


def SetHOMECmd(api, temp, isQueued=0):
    cmd = HOMECmd()
    cmd.temp = temp
    queuedCmdIndex = c_uint64(0)
    while(True):
        result = api.SetHOMECmd(byref(cmd), isQueued, byref(queuedCmdIndex))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [queuedCmdIndex.value]
    

def SetAutoLevelingCmd(api, controlFlag, precision, isQueued=0):
    cmd = AutoLevelingCmd()
    cmd.controlFlag = controlFlag
    cmd.precision = precision
    queuedCmdIndex = c_uint64(0)
    while(True):
        result = api.SetAutoLevelingCmd(byref(cmd), isQueued, byref(queuedCmdIndex))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [queuedCmdIndex.value]


def GetAutoLevelingResult(api):
    precision = c_float(0)
    while(True):
        result = api.GetAutoLevelingResult(byref(precision))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [precision.value]


def SetArmOrientation(api,  armOrientation, isQueued=0):
    queuedCmdIndex = c_uint64(0)
    while(True):
        result = api.SetArmOrientation(armOrientation, isQueued, byref(queuedCmdIndex))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [queuedCmdIndex.value]
    

def GetArmOrientation(api):
    armOrientation = c_int32(0)
    while(True):
        result = api.GetArmOrientation(byref(armOrientation))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [armOrientation.value]
    

def SetHHTTrigMode(api, hhtTrigMode):
    while(True):
        result = api.SetHHTTrigMode(hhtTrigMode)
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
        

def GetHHTTrigMode(api):
    hhtTrigMode = c_int(0)
    while(True):
        result = api.GetHHTTrigMode(byref(hhtTrigMode))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [hhtTrigMode.value]


def SetHHTTrigOutputEnabled(api, isEnabled):
    while(True):
        result = api.SetHHTTrigOutputEnabled(isEnabled)
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break


def GetHHTTrigOutputEnabled(api):
    isEnabled = c_int32(0)
    while(True):
        result = api.GetHHTTrigOutputEnabled(byref(isEnabled))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [isEnabled.value]

#
def GetHHTTrigOutput(api):
    isAvailable = c_int32(0)
    result = api.GetHHTTrigOutput(byref(isAvailable))
    if result != DobotCommunicate.DobotCommunicate_NoError or isAvailable.value == 0:
        return [False]
    return [True]
    

def SetEndEffectorParams(api, xBias, yBias, zBias, isQueued=0):
    param = EndTypeParams()
    param.xBias = xBias
    param.yBias = yBias
    param.zBias = zBias
    queuedCmdIndex = c_uint64(0)
    while(True):
        result = api.SetEndEffectorParams(byref(param),  isQueued,  byref(queuedCmdIndex))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [queuedCmdIndex.value]
        

def GetEndEffectorParams(api):
    param = EndTypeParams()
    while(True):
        result = api.GetEndEffectorParams(byref(param))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [param.xBias, param.yBias, param.zBias]
    

def SetEndEffectorLaser(api, enableCtrl,  on, isQueued=0):
    queuedCmdIndex = c_uint64(0)
    while(True):
        result = api.SetEndEffectorLaser(enableCtrl,  on,  isQueued,  byref(queuedCmdIndex))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [queuedCmdIndex.value]
        

def GetEndEffectorLaser(api):
    isCtrlEnabled = c_int(0)
    isOn = c_int(0)
    while(True):
        result = api.GetEndEffectorLaser(byref(isCtrlEnabled),  byref(isOn))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [isCtrlEnabled.value, isOn.value]
    

def SetEndEffectorSuctionCup(api, enableCtrl,  on, isQueued=0):
    queuedCmdIndex = c_uint64(0)
    while(True):
        result = api.SetEndEffectorSuctionCup(enableCtrl,  on,  isQueued,  byref(queuedCmdIndex))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [queuedCmdIndex.value]
        

def GetEndEffectorSuctionCup(api):
    enableCtrl = c_int(0)
    isOn = c_int(0)
    while(True):
        result = api.GetEndEffectorSuctionCup(byref(enableCtrl),  byref(isOn))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [isOn.value]
    

def SetEndEffectorGripper(api, enableCtrl,  on, isQueued=0):
    queuedCmdIndex = c_uint64(0)
    while(True):
        result = api.SetEndEffectorGripper(enableCtrl,  on,  isQueued,  byref(queuedCmdIndex))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [queuedCmdIndex.value]
        

def GetEndEffectorGripper(api):
    enableCtrl = c_int(0)
    isOn = c_int(0)
    while(True):
        result = api.GetEndEffectorGripper(byref(enableCtrl),  byref(isOn))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [isOn.value]


def SetJOGJointParams(api, j1Velocity, j1Acceleration, j2Velocity, j2Acceleration, j3Velocity, j3Acceleration, j4Velocity, j4Acceleration, isQueued=0):
    jogParam = JOGJointParams()
    jogParam.joint1Velocity = j1Velocity
    jogParam.joint1Acceleration = j1Acceleration
    jogParam.joint2Velocity = j2Velocity
    jogParam.joint2Acceleration = j2Acceleration
    jogParam.joint3Velocity = j3Velocity
    jogParam.joint3Acceleration = j3Acceleration
    jogParam.joint4Velocity = j4Velocity
    jogParam.joint4Acceleration = j4Acceleration
    queuedCmdIndex = c_uint64(0)
    while(True):
        result = api.SetJOGJointParams(byref(jogParam), isQueued, byref(queuedCmdIndex))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [queuedCmdIndex.value]


def GetJOGJointParams(api):
    param = JOGJointParams()
    while(True):
        result = api.GetJOGJointParams(byref(param))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [param.joint1Velocity, param.joint1Acceleration, param.joint2Velocity, param.joint2Acceleration, param.joint3Velocity, param.joint3Acceleration, param.joint4Velocity, param.joint4Acceleration]


def SetJOGCoordinateParams(api, xVelocity, xAcceleration, yVelocity, yAcceleration, zVelocity, zAcceleration, rVelocity, rAcceleration, isQueued=0):
    param = JOGCoordinateParams()
    param.xVelocity = xVelocity
    param.xAcceleration = xAcceleration
    param.yVelocity = yVelocity
    param.yAcceleration = yAcceleration
    param.zVelocity = zVelocity
    param.zAcceleration = zAcceleration
    param.rVelocity = rVelocity
    param.rAcceleration = rAcceleration
    queuedCmdIndex = c_uint64(0)
    while(True):
        result = api.SetJOGCoordinateParams(byref(param), isQueued, byref(queuedCmdIndex))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [queuedCmdIndex.value]


def GetJOGCoordinateParams(api):
    param = JOGCoordinateParams()
    while(True):
        result = api.GetJOGCoordinateParams(byref(param))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [param.xVelocity, param.xAcceleration, param.yVelocity, param.yVelocity, param.zVelocity, param.zAcceleration, param.rVelocity, param.rAcceleration]


def SetJOGLParams(api, velocity, acceleration, isQueued=0):
    param = JOGLParams()
    param.velocity = velocity
    param.acceleration = acceleration
    queuedCmdIndex = c_uint64(0)
    while(True):
        result = api.SetJOGLParams(byref(param), isQueued, byref(queuedCmdIndex))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [queuedCmdIndex.value]
    

def GetJOGLParams(api):
    param = JOGLParams()
    while(True):
        result = api.GetJOGLParams(byref(param))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [param.velocity,  param.acceleration]


def SetJOGCommonParams(api, value_velocityratio, value_accelerationratio, isQueued=0):
    param = JOGCommonParams()
    param.velocityRatio = value_velocityratio
    param.accelerationRatio = value_accelerationratio
    queuedCmdIndex = c_uint64(0)
    while(True):
        result = api.SetJOGCommonParams(byref(param), isQueued, byref(queuedCmdIndex))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [queuedCmdIndex.value]


def GetJOGCommonParams(api):
    param = JOGCommonParams()
    while(True):
        result = api.GetJOGCommonParams(byref(param))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [param.velocityRatio, param.accelerationRatio]


def SetJOGCmd(api, isJoint, cmd, isQueued=0):
    cmdParam = JOGCmd()
    cmdParam.isJoint = isJoint
    cmdParam.cmd = cmd
    queuedCmdIndex = c_uint64(0)
    while(True):
        result = api.SetJOGCmd(byref(cmdParam), isQueued, byref(queuedCmdIndex))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [queuedCmdIndex.value]


def SetPTPJointParams(api, j1Velocity, j1Acceleration, j2Velocity, j2Acceleration, j3Velocity, j3Acceleration, j4Velocity, j4Acceleration, isQueued=0):
    pbParam = PTPJointParams()
    pbParam.joint1Velocity = j1Velocity
    pbParam.joint1Acceleration = j1Acceleration
    pbParam.joint2Velocity = j2Velocity
    pbParam.joint2Acceleration = j2Acceleration
    pbParam.joint3Velocity = j3Velocity
    pbParam.joint3Acceleration = j3Acceleration
    pbParam.joint4Velocity = j4Velocity
    pbParam.joint4Acceleration = j4Acceleration
    queuedCmdIndex = c_uint64(0)
    while(True):
        result = api.SetPTPJointParams(byref(pbParam), isQueued, byref(queuedCmdIndex))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [queuedCmdIndex.value]


def GetPTPJointParams(api):
    pbParam = PTPJointParams()
    while(True):
        result = api.GetPTPJointParams(byref(pbParam))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [pbParam.joint1Velocity,pbParam.joint1Acceleration,pbParam.joint2Velocity,pbParam.joint2Acceleration,pbParam.joint3Velocity,pbParam.joint3Acceleration,pbParam.joint4Velocity,pbParam.joint4Acceleration]


def SetPTPCoordinateParams(api, xyzVelocity, xyzAcceleration, rVelocity,  rAcceleration,  isQueued=0):
    pbParam = PTPCoordinateParams()
    pbParam.xyzVelocity = xyzVelocity
    pbParam.rVelocity = rVelocity
    pbParam.xyzAcceleration = xyzAcceleration
    pbParam.rAcceleration = rAcceleration
    queuedCmdIndex = c_uint64(0)
    while(True):
        result = api.SetPTPCoordinateParams(byref(pbParam), isQueued, byref(queuedCmdIndex))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [queuedCmdIndex.value]


def GetPTPCoordinateParams(api):
    pbParam = PTPCoordinateParams()
    while(True):
        result = api.GetPTPCoordinateParams(byref(pbParam))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [pbParam.xyzVelocity, pbParam.rVelocity, pbParam.xyzAcceleration, pbParam.rAcceleration]
    

def SetPTPLParams(api, velocity, acceleration, isQueued=0):
    param = PTPLParams()
    param.velocity = velocity
    param.acceleration = acceleration
    queuedCmdIndex = c_uint64(0)
    while(True):
        result = api.SetPTPLParams(byref(param), isQueued, byref(queuedCmdIndex))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [queuedCmdIndex.value]
    

def GetPTPLParams(api):
    param = PTPLParams()
    while(True):
        result = api.GetPTPLParams(byref(param))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [param.velocity,  param.acceleration]
    

def SetPTPJumpParams(api, jumpHeight, zLimit, isQueued=0):
    pbParam = PTPJumpParams()
    pbParam.jumpHeight = jumpHeight
    pbParam.zLimit = zLimit
    queuedCmdIndex = c_uint64(0)
    while(True):
        result = api.SetPTPJumpParams(byref(pbParam), isQueued, byref(queuedCmdIndex))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [queuedCmdIndex.value]


def GetPTPJumpParams(api):
    pbParam = PTPJumpParams()
    while(True):
        result = api.GetPTPJumpParams(byref(pbParam))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [pbParam.jumpHeight, pbParam.zLimit]


def SetPTPCommonParams(api, velocityRatio, accelerationRatio, isQueued=0):
    pbParam = PTPCommonParams()
    pbParam.velocityRatio = velocityRatio
    pbParam.accelerationRatio = accelerationRatio
    queuedCmdIndex = c_uint64(0)
    while(True):
        result = api.SetPTPCommonParams(byref(pbParam), isQueued, byref(queuedCmdIndex))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [queuedCmdIndex.value]


def GetPTPCommonParams(api):
    pbParam = PTPCommonParams()
    while(True):
        result = api.GetPTPCommonParams(byref(pbParam ))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [pbParam.velocityRatio, pbParam.accelerationRatio]
    

def SetPTPCmd(api, ptpMode, x, y, z, rHead, isQueued=0):
    cmd = PTPCmd()
    cmd.ptpMode=ptpMode
    cmd.x=x
    cmd.y=y
    cmd.z=z
    cmd.rHead=rHead
    queuedCmdIndex = c_uint64(0)
    while(True):
        result = api.SetPTPCmd(byref(cmd), isQueued, byref(queuedCmdIndex))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(2)
            continue
        break
    return [queuedCmdIndex.value]
    

def SetPTPWithLCmd(api, ptpMode, x, y, z, rHead, l, isQueued=0):
    cmd = PTPWithLCmd()
    cmd.ptpMode=ptpMode
    cmd.x=x
    cmd.y=y
    cmd.z=z
    cmd.rHead=rHead
    cmd.l = l
    queuedCmdIndex = c_uint64(0)
    while(True):
        result = api.SetPTPWithLCmd(byref(cmd), isQueued, byref(queuedCmdIndex))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(2)
            continue
        break
    return [queuedCmdIndex.value]
    

def SetCPRHoldEnable(api, isEnable):
    while(True):
        result = api.SetCPRHoldEnable(c_bool(isEnable))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break


def GetCPRHoldEnable(api):
    isEnable = c_bool(False)
    while(True):
        result = api.GetCPRHoldEnable(byref(isEnable))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [isEnable.value]
    

def SetCPParams(api, planAcc, juncitionVel, acc, realTimeTrack = 0,  isQueued=0):
    parm = CPParams()
    parm.planAcc = planAcc
    parm.juncitionVel = juncitionVel
    parm.acc = acc
    parm.realTimeTrack = realTimeTrack
    queuedCmdIndex = c_uint64(0)
    while(True):
        result = api.SetCPParams(byref(parm), isQueued, byref(queuedCmdIndex))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [queuedCmdIndex.value]


def GetCPParams(api):
    parm = CPParams()
    while(True):
        result = api.GetCPParams(byref(parm))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [parm.planAcc, parm.juncitionVel, parm.acc, parm.realTimeTrack]


def SetCPCmd(api, cpMode, x, y, z, velocity, isQueued=0):
    cmd = CPCmd()
    cmd.cpMode = cpMode
    cmd.x = x
    cmd.y = y
    cmd.z = z
    cmd.velocity = velocity
    queuedCmdIndex = c_uint64(0)
    while(True):
        result = api.SetCPCmd(byref(cmd), isQueued, byref(queuedCmdIndex))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(2)
            continue
        break
    return [queuedCmdIndex.value]
    

def SetCPCommonParams(api, velocityRatio, accelerationRatio, isQueued=0):
    pbParam = CPCommonParams()
    pbParam.velocityRatio = velocityRatio
    pbParam.accelerationRatio = accelerationRatio
    queuedCmdIndex = c_uint64(0)
    while(True):
        result = api.SetCPCommonParams(byref(pbParam), isQueued, byref(queuedCmdIndex))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [queuedCmdIndex.value]


def GetCPCommonParams(api):
    pbParam = CPCommonParams()
    while(True):
        result = api.GetCPCommonParams(byref(pbParam ))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [pbParam.velocityRatio, pbParam.accelerationRatio]
    

def SetCPLECmd(api, cpMode, x, y, z, power, isQueued=0):
    cmd = CPCmd()
    cmd.cpMode = cpMode
    cmd.x = x
    cmd.y = y
    cmd.z = z
    cmd.velocity = power
    queuedCmdIndex = c_uint64(0)
    while(True):
        result = api.SetCPLECmd(byref(cmd), isQueued, byref(queuedCmdIndex))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(2)
            continue
        break
    return [queuedCmdIndex.value]
    

def SetARCParams(api,  xyzVelocity, rVelocity, xyzAcceleration, rAcceleration,  isQueued=0):
    param = ARCParams()
    param.xyzVelocity = xyzVelocity
    param.rVelocity = rVelocity
    param.xyzAcceleration = xyzAcceleration
    param.rAcceleration = rAcceleration
    queuedCmdIndex = c_uint64(0)
    while(True):
        result = api.SetARCParams(byref(param), isQueued, byref(queuedCmdIndex))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [queuedCmdIndex.value]
        

def GetARCParams(api):
    parm = ARCParams()
    while(True):
        result = api.GetARCParams(byref(parm))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [parm.xyzVelocity, parm.rVelocity, parm.xyzAcceleration, parm.rAcceleration]
    

def SetARCCmd(api, cirPoint, toPoint,  isQueued=0):
    cmd = ARCCmd()
    cmd.cirPoint.x = cirPoint[0];cmd.cirPoint.y = cirPoint[1];cmd.cirPoint.z = cirPoint[2];cmd.cirPoint.rHead = cirPoint[3]
    cmd.toPoint.x = toPoint[0];cmd.toPoint.y = toPoint[1];cmd.toPoint.z = toPoint[2];cmd.toPoint.rHead = toPoint[3]
    queuedCmdIndex = c_uint64(0)
    while(True):
        result = api.SetARCCmd(byref(cmd), isQueued, byref(queuedCmdIndex))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [queuedCmdIndex.value]
    

def SetCircleCmd(api, cirPoint, toPoint,  isQueued=0):
    cmd = CircleCmd()
    cmd.cirPoint.x = cirPoint[0];cmd.cirPoint.y = cirPoint[1];cmd.cirPoint.z = cirPoint[2];cmd.cirPoint.rHead = cirPoint[3]
    cmd.toPoint.x = toPoint[0];cmd.toPoint.y = toPoint[1];cmd.toPoint.z = toPoint[2];cmd.toPoint.rHead = toPoint[3]
    queuedCmdIndex = c_uint64(0)
    while(True):
        result = api.SetCircleCmd(byref(cmd), isQueued, byref(queuedCmdIndex))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [queuedCmdIndex.value]
    

def SetARCCommonParams(api, velocityRatio, accelerationRatio, isQueued=0):
    pbParam = ARCCommonParams()
    pbParam.velocityRatio = velocityRatio
    pbParam.accelerationRatio = accelerationRatio
    queuedCmdIndex = c_uint64(0)
    while(True):
        result = api.SetARCCommonParams(byref(pbParam), isQueued, byref(queuedCmdIndex))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [queuedCmdIndex.value]


def GetARCCommonParams(api):
    pbParam = ARCCommonParams()
    while(True):
        result = api.GetARCCommonParams(byref(pbParam ))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [pbParam.velocityRatio, pbParam.accelerationRatio]


def SetWAITCmd(api, waitTime, isQueued=0):
    param = WAITCmd()
    param.waitTime = int(waitTime)
    queuedCmdIndex = c_uint64(0)
    while(True):
        result = api.SetWAITCmd(byref(param), isQueued, byref(queuedCmdIndex))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [queuedCmdIndex.value]


def SetTRIGCmd(api, address, mode,  condition,  threshold,  isQueued=0):
    param = TRIGCmd()
    param.address = address
    param.mode = mode
    param.condition = condition
    param.threshold = threshold
    queuedCmdIndex = c_uint64(0)
    while(True):
        result = api.SetTRIGCmd(byref(param), isQueued, byref(queuedCmdIndex))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [queuedCmdIndex.value]


def SetIOMultiplexing(api, address, multiplex, isQueued=0):
    param = IOMultiplexing()
    param.address = address
    param.multiplex = multiplex
    queuedCmdIndex = c_uint64(0)
    while(True):
        result = api.SetIOMultiplexing(byref(param), isQueued, byref(queuedCmdIndex))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [queuedCmdIndex.value]


def GetIOMultiplexing(api,  addr):
    param = IOMultiplexing()
    param.address = addr
    while(True):
        result = api.GetIOMultiplexing(byref(param))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [param.multiplex]


def SetIODO(api, address, level, isQueued=0):
    param = IODO()
    param.address = address
    param.level = level
    queuedCmdIndex = c_uint64(0)
    while(True):
        result = api.SetIODO(byref(param), isQueued, byref(queuedCmdIndex))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [queuedCmdIndex.value]


def GetIODO(api,  addr):
    param = IODO()
    param.address = addr
    while(True):
        result = api.GetIODO(byref(param))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [param.level]


def SetIOPWM(api, address, frequency, dutyCycle,  isQueued=0):
    param = IOPWM()
    param.address = address
    param.frequency = frequency
    param.dutyCycle = dutyCycle
    queuedCmdIndex = c_uint64(0)
    while(True):
        result = api.SetIOPWM(byref(param), isQueued, byref(queuedCmdIndex))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [queuedCmdIndex.value]


def GetIOPWM(api,  addr):
    param = IOPWM()
    param.address = addr
    while(True):
        result = api.GetIOPWM(byref(param))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [param.frequency,  param.dutyCycle]


def GetIODI(api,  addr):
    param = IODI()
    param.address = addr
    while(True):
        result = api.GetIODI(byref(param))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [param.level]
    

def SetEMotor(api, index, isEnabled, speed,  isQueued=0):
    emotor = EMotor()
    emotor.index = index
    emotor.isEnabled = isEnabled
    emotor.speed = speed
    queuedCmdIndex = c_uint64(0)
    while(True):
        result = api.SetEMotor(byref(emotor), isQueued, byref(queuedCmdIndex))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [queuedCmdIndex.value]
    

def SetEMotorS(api, index, isEnabled, speed, distance,  isQueued=0):
    emotorS = EMotorS()
    emotorS.index = index
    emotorS.isEnabled = isEnabled
    emotorS.speed = speed
    emotorS.distance = distance
    queuedCmdIndex = c_uint64(0)
    while(True):
        result = api.SetEMotorS(byref(emotorS), isQueued, byref(queuedCmdIndex))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [queuedCmdIndex.value]


def GetIOADC(api,  addr):
    param = IOADC()
    param.address = addr
    while(True):
        result = api.GetIOADC(byref(param))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [param.value]


def SetAngleSensorStaticError(api,  rearArmAngleError, frontArmAngleError):
    c_rearArmAngleError = c_float(rearArmAngleError)
    c_frontArmAngleError = c_float(frontArmAngleError)
    while(True):
        result = api.SetAngleSensorStaticError(c_rearArmAngleError, c_frontArmAngleError)
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
        

def GetAngleSensorStaticError(api):
    rearArmAngleError = c_float(0)
    frontArmAngleError = c_float(0)
    while(True):
        result = api.GetAngleSensorStaticError(byref(rearArmAngleError),  byref(frontArmAngleError))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [rearArmAngleError.value, frontArmAngleError.value]
    

def SetAngleSensorCoef(api,  rearArmAngleCoef, frontArmAngleCoef):
    c_rearArmAngleCoef = c_float(rearArmAngleCoef)
    c_frontArmAngleCoef = c_float(frontArmAngleCoef)
    while(True):
        result = api.SetAngleSensorCoef(c_rearArmAngleCoef, c_frontArmAngleCoef)
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
        

def GetAngleSensorCoef(api):
    rearArmAngleCoef = c_float(0)
    frontArmAngleCoef = c_float(0)
    while(True):
        result = api.GetAngleSensorCoef(byref(rearArmAngleCoef),  byref(frontArmAngleCoef))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [rearArmAngleCoef.value, frontArmAngleCoef.value]


def SetBaseDecoderStaticError(api,  baseDecoderError):
    c_baseDecoderError = c_float(baseDecoderError)
    while(True):
        result = api.SetBaseDecoderStaticError(c_baseDecoderError)
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    

def GetBaseDecoderStaticError(api):
    baseDecoderError = c_float(0)
    while(True):
        result = api.GetBaseDecoderStaticError(byref(baseDecoderError))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [baseDecoderError.value]

"""
WIFI
JoMar 
20160906
"""

def GetWIFIConnectStatus(api):
    isConnected = c_bool(0)
    while(True):
        if not QuitDobotApiFlag:
            break
        result = api.GetWIFIConnectStatus(byref(isConnected))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [isConnected.value]

def SetWIFIConfigMode(api,  enable):
    while(True):
        if not QuitDobotApiFlag:
            break
        result = api.SetWIFIConfigMode(enable)
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    

def GetWIFIConfigMode(api):
    isEnabled = c_bool(0)
    while(True):
        if not QuitDobotApiFlag:
            break
        result = api.GetWIFIConfigMode(byref(isEnabled))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [isEnabled.value]
    

def SetWIFISSID(api,  ssid):
    szPara = create_string_buffer(len(ssid))
    szPara.raw = ssid.encode("utf-8")
    while(True):
        if not QuitDobotApiFlag:
            break
        result = api.SetWIFISSID(szPara)
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    

def GetWIFISSID(api):
    szPara = create_string_buffer(100)
    while(True):
        if not QuitDobotApiFlag:
            break
        result = api.GetWIFISSID(szPara,  25)
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    ssid = szPara.value.decode("utf-8") 
    return [ssid]
    

def SetWIFIPassword(api,  password):
    szPara = create_string_buffer(25)
    szPara.raw = password.encode("utf-8")
    while(True):
        if not QuitDobotApiFlag:
            break
        result = api.SetWIFIPassword(szPara)
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
        

def GetWIFIPassword(api):
    szPara = create_string_buffer(25)  
    while(True):
        if not QuitDobotApiFlag:
            break
        result = api.GetWIFIPassword(szPara,  25)
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    password = szPara.value.decode("utf-8") 
    return [password]
    

def SetWIFIIPAddress(api,  dhcp,  addr1,  addr2,  addr3,  addr4):
    wifiIPAddress = WIFIIPAddress()
    wifiIPAddress.dhcp = dhcp
    wifiIPAddress.addr1 = addr1
    wifiIPAddress.addr2 = addr2
    wifiIPAddress.addr3 = addr3
    wifiIPAddress.addr4 = addr4
    while(True):
        if not QuitDobotApiFlag:
            break
        result = api.SetWIFIIPAddress(byref(wifiIPAddress))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
        

def GetWIFIIPAddress(api):
    wifiIPAddress = WIFIIPAddress()
    while(True):
        if not QuitDobotApiFlag:
            break
        result = api.GetWIFIIPAddress(byref(wifiIPAddress))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [c_uint8(wifiIPAddress.dhcp).value,  c_uint8(wifiIPAddress.addr1).value,  c_uint8(wifiIPAddress.addr2).value,   c_uint8(wifiIPAddress.addr3).value,  c_uint8(wifiIPAddress.addr4).value]
    

def SetWIFINetmask(api, addr1,  addr2,  addr3,  addr4):
    wifiNetmask = WIFINetmask()
    wifiNetmask.addr1 = addr1
    wifiNetmask.addr2 = addr2
    wifiNetmask.addr3 = addr3
    wifiNetmask.addr4 = addr4
    while(True):
        if not QuitDobotApiFlag:
            break
        result = api.SetWIFINetmask(byref(wifiNetmask))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
        

def GetWIFINetmask(api):
    wifiNetmask = WIFINetmask()
    while(True):
        if not QuitDobotApiFlag:
            break
        result = api.GetWIFINetmask(byref(wifiNetmask))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [c_uint8(wifiNetmask.addr1).value,  c_uint8(wifiNetmask.addr2).value,  c_uint8(wifiNetmask.addr3).value,  c_uint8(wifiNetmask.addr4).value]
    

def SetWIFIGateway(api, addr1,  addr2,  addr3,  addr4):
    wifiGateway = WIFIGateway()
    wifiGateway.addr1 = addr1
    wifiGateway.addr2 = addr2
    wifiGateway.addr3 = addr3
    wifiGateway.addr4 = addr4
    while(True):
        if not QuitDobotApiFlag:
            break
        result = api.SetWIFIGateway(byref(wifiGateway))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break


def GetWIFIGateway(api):
    wifiGateway = WIFIGateway()
    while(True):
        if not QuitDobotApiFlag:
            break
        result = api.GetWIFIGateway(byref(wifiGateway))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [c_uint8(wifiGateway.addr1).value,  c_uint8(wifiGateway.addr2).value,  c_uint8(wifiGateway.addr3).value,  c_uint8(wifiGateway.addr4).value]
    

def SetWIFIDNS(api, addr1,  addr2,  addr3,  addr4):
    wifiDNS = WIFIDNS()
    wifiDNS.addr1 = addr1
    wifiDNS.addr2 = addr2
    wifiDNS.addr3 = addr3
    wifiDNS.addr4 = addr4
    while(True):
        if not QuitDobotApiFlag:
            break
        result = api.SetWIFIDNS(byref(wifiDNS))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break


def GetWIFIDNS(api):
    wifiDNS = WIFIDNS()
    while(True):
        if not QuitDobotApiFlag:
            break
        result = api.GetWIFIDNS(byref(wifiDNS))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [c_uint8(wifiDNS.addr1).value,  c_uint8(wifiDNS.addr2).value,  c_uint8(wifiDNS.addr3).value,  c_uint8(wifiDNS.addr4).value]


def SetColorSensor(api, isEnable, colorPort, version=0):
    enable = c_bool(isEnable)
    port = c_uint8(colorPort)
    version = c_uint8(version)
    while(True):
        result = api.SetColorSensor(enable, port, version)
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    

def GetColorSensor(api):
    r = c_ubyte(0)
    g = c_ubyte(0)
    b = c_ubyte(0)
    while(True):
        result = api.GetColorSensor(byref(r),  byref(g),  byref(b))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [r.value, g.value, b.value]
    

def SetInfraredSensor(api,  isEnable, infraredPort, version=0):
    enable = c_bool(isEnable)
    port = c_uint8(infraredPort)
    version = c_uint8(version)
    while(True):
        result = api.SetInfraredSensor(enable, port, version)
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    

def GetInfraredSensor(api, infraredPort):
    port = c_uint8(infraredPort)
    value = c_ubyte(0)
    
    while(True):
        result = api.GetInfraredSensor(port,  byref(value))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [value.value]
    

def SetLostStepParams(api, threshold, isQueued=0):
    queuedCmdIndex = c_uint64(0)
    t = c_float(threshold)
    while(True):
        result = api.SetLostStepParams(t, isQueued, byref(queuedCmdIndex))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [queuedCmdIndex.value]


def SetLostStepCmd(api, isQueued=0):
    queuedCmdIndex = c_uint64(0)
    while(True):
        result = api.SetLostStepCmd(isQueued, byref(queuedCmdIndex))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [queuedCmdIndex.value]
    

def GetUART4PeripheralsType(api):
    type = c_uint8(0)
    while(True):
        result = api.GetUART4PeripheralsType(byref(type))
        if result != DobotCommunicate.DobotCommunicate_NoError:
            dSleep(5)
            continue
        break
    return [type.value]
    
    
        
##################  Ex扩展函数，该套函数会检测每一条指令运行完毕  ##################
def GetPoseEx(api,  index):
    if index == 0:
        ret = GetDeviceWithL(api)
        if not ret:
            print("Dobot is not in L model")
            return
            
        lr = GetPoseL(api)
        return round(lr[0],  4)
        
    pos = GetPose(api)
    return round(pos[index-1],  4)
    
def SetHOMECmdEx(api,  temp,  isQueued=0):
    ret = SetHOMECmd(api, temp,  isQueued)
    queuedCmdIndex = c_uint64(0)
    while(True):
        result = api.GetQueuedCmdCurrentIndex(byref(queuedCmdIndex))
        if result == DobotCommunicate.DobotCommunicate_NoError and ret[0] <= queuedCmdIndex.value:
            break
        #延时太短的话按reset键后不能断开连接
        dSleep(100)
        
def SetWAITCmdEx(api, waitTime, isQueued=0):
    #ret = SetWAITCmd(api, waitTime, isQueued)
    #while(True):
        # if not QuitDobotApiFlag:
        #     break
    #   if ret[0] <= GetQueuedCmdCurrentIndex(api)[0]:
    #        break
    dSleep(waitTime * 1000)
    
def SetEndEffectorParamsEx(api, xBias, yBias, zBias, isQueued=0):
    ret = SetEndEffectorParams(api, xBias, yBias, zBias, isQueued)
    while(True):
        if ret[0] <= GetQueuedCmdCurrentIndex(api)[0]:
            break
        dSleep(5)
        
def SetPTPJointParamsEx(api, j1Velocity, j1Acceleration, j2Velocity, j2Acceleration, j3Velocity, j3Acceleration, j4Velocity, j4Acceleration, isQueued=0):
    ret = SetPTPJointParams(api, j1Velocity, j1Acceleration, j2Velocity, j2Acceleration, j3Velocity, j3Acceleration, j4Velocity, j4Acceleration, isQueued)
    while(True):
        if ret[0] <= GetQueuedCmdCurrentIndex(api)[0]:
            break
        dSleep(5)
        
def SetPTPCoordinateParamsEx(api, xyzVelocity, xyzAcceleration, rVelocity,  rAcceleration,  isQueued=0):
    ret = SetPTPCoordinateParams(api, xyzVelocity, xyzAcceleration, rVelocity,  rAcceleration,  isQueued)
    while(True):
        if ret[0] <= GetQueuedCmdCurrentIndex(api)[0]:
            break
        dSleep(5)

def SetPTPLParamsEx(api, lVelocity, lAcceleration, isQueued=0):
    ret = GetDeviceWithL(api)
    if not ret:
        print("Dobot is not in L model")
        return
    
    ret = SetPTPLParams(api, lVelocity, lAcceleration, isQueued)
    while(True):
        if ret[0] <= GetQueuedCmdCurrentIndex(api)[0]:
            break
        dSleep(5)
        
def SetPTPCommonParamsEx(api, velocityRatio, accelerationRatio, isQueued=0):
    ret = SetPTPCommonParams(api, velocityRatio, accelerationRatio, isQueued)
    while(True):
        if ret[0] <= GetQueuedCmdCurrentIndex(api)[0]:
            break
        dSleep(5)
        
def SetPTPJumpParamsEx(api, jumpHeight, maxJumpHeight, isQueued=0):
    ret = SetPTPJumpParams(api, jumpHeight, maxJumpHeight, isQueued)
    while(True):
        if ret[0] <= GetQueuedCmdCurrentIndex(api)[0]:
            break
        dSleep(5)
        
def SetPTPCmdEx(api, ptpMode, x, y, z, rHead, isQueued=0):
    ret = SetPTPCmd(api, ptpMode, x, y, z, rHead, isQueued)
    while(True):
        if ret[0] <= GetQueuedCmdCurrentIndex(api)[0]:
            break
        dSleep(5)
    
def SetIOMultiplexingEx(api, address, multiplex, isQueued=0):
    ret = SetIOMultiplexing(api, address, multiplex, isQueued)
    while(True):
        if ret[0] <= GetQueuedCmdCurrentIndex(api)[0]:
            break
        dSleep(5)
        
def SetEndEffectorSuctionCupEx(api, enableCtrl,  on, isQueued=0):
    ret = SetEndEffectorSuctionCup(api, enableCtrl,  on, isQueued)
    while(True):
        if ret[0] <= GetQueuedCmdCurrentIndex(api)[0]:
            break
        dSleep(5)

def SetEndEffectorGripperEx(api, enableCtrl,  on, isQueued=0):
    ret = SetEndEffectorGripper(api, enableCtrl,  on, isQueued)
    while(True):
        if ret[0] <= GetQueuedCmdCurrentIndex(api)[0]:
            break
        dSleep(5)
        
def SetEndEffectorLaserEx(api, enableCtrl, power, isQueued=0):
    SetIOMultiplexingEx(api, 2,  1, isQueued)
    SetIOMultiplexingEx(api, 4,  2, isQueued)
    SetIODOEx(api, 2, enableCtrl, isQueued)
    SetIOPWMEx(api, 4, 10000, power, isQueued)

def SetIODOEx(api, address, level, isQueued=0):
    ret = SetIODO(api, address, level, isQueued)
    while(True):
        if ret[0] <= GetQueuedCmdCurrentIndex(api)[0]:
            break
        dSleep(5)
        
def SetEMotorEx(api, index, isEnabled, speed,  isQueued=0):
    ret = SetEMotor(api, index, isEnabled, speed,  isQueued)
    while(True):
        if ret[0] <= GetQueuedCmdCurrentIndex(api)[0]:
            break
        dSleep(5)
    
def SetEMotorSEx(api, index, isEnabled, speed, distance,  isQueued=0):
    ret = SetEMotorS(api, index, isEnabled, speed, distance,   isQueued)
    while(True):
        if ret[0] <= GetQueuedCmdCurrentIndex(api)[0]:
            break
        dSleep(5)
    
def SetIOPWMEx(api, address, frequency, dutyCycle,  isQueued=0):
    while(True):
        ret = SetIOPWM(api, address, frequency, dutyCycle,  isQueued)
        if ret[0] <= GetQueuedCmdCurrentIndex(api)[0]:
            break
        dSleep(5)

def SetPTPWithLCmdEx(api, ptpMode, x, y, z, rHead,  l, isQueued=0):
    ret = GetDeviceWithL(api)
    if not ret:
        print("Dobot is not in L model")
        return
        
    ret = SetPTPWithLCmd(api, ptpMode, x, y, z, rHead, l,  isQueued)
    while(True):
        if ret[0] <= GetQueuedCmdCurrentIndex(api)[0]:
            break
        dSleep(5)
        
def GetColorSensorEx(api,  index):
    result = GetColorSensor(api)
    return result[index]
    
def SetAutoLevelingCmdEx(api, controlFlag, precision, isQueued=1):
    index = SetAutoLevelingCmd(api, controlFlag, precision, isQueued)[0]
    while(True):
        if index <= GetQueuedCmdCurrentIndex(api)[0]:
            break
        dSleep(5)
    
def SetLostStepCmdEx(api, isQueued=1):
    ret = SetLostStepCmd(api, isQueued)
    while(True):
        if ret[0] <= GetQueuedCmdCurrentIndex(api)[0]:
            break
        dSleep(5)