# Dobot program generated from RoboDK post processor
import threading
import sys
import os
from packaging import version

# Communication port (usually serial port)
COM_CONNECT = "COM10"
print("Connecting to: " + COM_CONNECT)

# Library path: Important, Python 3.5 or later must be used. Make sure to link suitable binaries (32 vs 64 bit)
DOBOT_LIBRARY_PATH = r"""C:\RoboDK\bin"""
#DOBOT_LIBRARY_PATH = r"""C:/Users/UserName/Desktop/DobotDll"""

sys.path.append(os.path.abspath(DOBOT_LIBRARY_PATH)) # temporarily add path to POSTS folder
os.environ['PATH'] = DOBOT_LIBRARY_PATH + os.pathsep + os.environ['PATH']

python_min_version = "3.6.0"
if version.parse(str(sys.version_info[0]) + "." + str(sys.version_info[1]) + "." + str(sys.version_info[2])) < version.parse(python_min_version):
    input("Invalid Python version. Use Python 3.6.0 or later.")


    import DobotDllType as dType
CON_STR = {
    dType.DobotConnect.DobotConnect_NoError:  "DobotConnect_NoError",
    dType.DobotConnect.DobotConnect_NotFound: "DobotConnect_NotFound",
    dType.DobotConnect.DobotConnect_Occupied: "DobotConnect_Occupied"
}

# Load Dll
api = dType.load()

# Connect to Dobot
[state,  fwType,  version, dobotId] = dType.ConnectDobot(api, COM_CONNECT, 115200)
print("Connection status: ", CON_STR[state])
print("Connected robot: ", dobotId)
sys.stdout.flush()

#Tools
def suctionOn():
    dType.SetEndEffectorSuctionCup(api, dobotId, 1,  1, 1)

def suctionOff():
    dType.SetEndEffectorSuctionCup(api, dobotId, 1,  0, 1)
    
if (state != dType.DobotConnect.DobotConnect_NoError):
    #Disconnect Dobot
    dType.DisconnectDobot(api,dobotId)
    raise Exception("Connection problems (" + CON_STR[state] + ") Are you using the robot with another application?")
    quit()



# Clean Command Queued
dType.SetQueuedCmdClear(api,dobotId)

# Async Motion Params Setting
dType.SetHOMEParams(api,dobotId, 200, 200, 200, 200, isQueued = 1)
dType.SetPTPJointParams(api,dobotId, 200, 200, 200, 200, 200, 200, 200, 200, isQueued = 1)
dType.SetPTPCommonParams(api,dobotId, 100, 100, isQueued = 1)


# Async Home (only required after rebooting the controller)
#dType.SetHOMECmd(api,dobotId, temp=0, isQueued=1)

def Home():
    """Set the Dobot Magician back to its home position to get a correct reference position. 
    Perform the homing operation after rebooting the controller or the stepper motors lost steps"""
    dType.SetHOMECmd(api,dobotId, temp=0, isQueued=1)

# Main program definition
def SuckTest():
    '''Main procedure'''
    print('Sending program SuckTest ...')
    # Program generated by RoboDK v4.0.2 for Dobot Magician on 27/01/2020 15:10:14
    # Using nominal kinematics.
    dType.SetPTPCmd(api,dobotId, dType.PTPMode.PTPMOVJANGLEMode, -0.000000, 0.000000, 0.000000, -0.000000, isQueued=1)
    dType.SetPTPCmd(api,dobotId, dType.PTPMode.PTPMOVJANGLEMode, 19.810859, 9.362202, 20.012996, 19.810859, isQueued=1)
    suctionOn()
    dType.SetWAITCmd(api,dobotId, 0.5, 1)
    dType.SetPTPCmd(api,dobotId, dType.PTPMode.PTPMOVJANGLEMode, 0.926639, 0.353042, -5.997554, 0.926639, isQueued=1)
    print('Program SuckTest sent')
    sys.stdout.flush()
    print('Running program SuckTest on robot...')
    sys.stdout.flush()

    # Save the index command of the last command (set a short pause)
    lastIndex = dType.SetWAITCmd(api,dobotId, 0.1, isQueued = 1)[0]
    
    # Start to Execute Queued Commands
    dType.SetQueuedCmdStartExec(api,dobotId)
    
    # Wait until execution is done by verifying current command
    while lastIndex > dType.GetQueuedCmdCurrentIndex(api,dobotId)[0]:
        dType.dSleep(100)
    
    # Stop executing commands
    dType.SetQueuedCmdStopExec(api,dobotId)
    # Clear all queued commands
    dType.SetQueuedCmdClear(api,dobotId)
    print('Program SuckTest Finished')
    sys.stdout.flush()


# Main program call: set as a loop
while True:
    SuckTest()
    break

#Disconnect Dobot
dType.DisconnectDobot(api,dobotId)

# Notify user before closing:
print("Done!")
input("Press any key to close.")