# 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()

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)

# Tool Functions --ADDED SECTION
def suctionOn():
    dType.SetEndEffectorSuctionCup(api, 1, 1)
def suctionOff():
    dType.SetEndEffectorSuctionCup(api, 0, 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 MoveandSuck():
    '''Main procedure'''
    print('Sending program MoveandSuck ...')
    # Program generated by RoboDK v4.0.2 for Dobot Magician on 20/12/2019 10:45:13
    # Using nominal kinematics.
    dType.SetPTPCmd(api,dobotId, dType.PTPMode.PTPMOVJANGLEMode, -2.773664, 0.236693, 3.743996, -2.773664, isQueued=1)
    dType.SetPTPCmd(api,dobotId, dType.PTPMode.PTPMOVJANGLEMode, 46.620771, 47.381159, 14.213205, 46.620771, isQueued=1)
    dType.SetPTPCmd(api,dobotId, dType.PTPMode.PTPMOVLANGLEMode, 46.620771, 61.927238, 33.397715, 46.620771, isQueued=1)
    suctionOn()
    dType.SetPTPCmd(api,dobotId, dType.PTPMode.PTPMOVJANGLEMode, -2.773664, 0.236693, 3.743996, -2.773664, isQueued=1)
    print('Program MoveandSuck sent')
    sys.stdout.flush()
    print('Running program MoveandSuck 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 MoveandSuck Finished')
    sys.stdout.flush()


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

#Disconnect Dobot
dType.DisconnectDobot(api,dobotId)

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