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

# Communication port (usually serial port)
COM_CONNECT = "192.168.5.1"
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\Posts\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)

# 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 Prog1():
    '''Main procedure'''
    print('Sending program Prog1 ...')
    # Program generated by RoboDK v5.3.0 for Dobot CR5 on 22/11/2021 17:15:58
    # Using nominal kinematics.
    dType.SetPTPCmd(api,dobotId, dType.PTPMode.PTPMOVJANGLEMode, 0.000000, 0.000000, -90.000000, 90.000000, 90.000000, -0.000000, isQueued=1)
    dType.SetPTPCmd(api,dobotId, dType.PTPMode.PTPMOVJANGLEMode, -18.225600, -8.799280, -80.394100, 89.193400, 108.226000, -0.000000, isQueued=1)
    dType.SetPTPCmd(api,dobotId, dType.PTPMode.PTPMOVJANGLEMode, -18.866400, -17.673000, -106.369000, 124.042000, 108.866000, -0.000000, isQueued=1)
    dType.SetPTPCmd(api,dobotId, dType.PTPMode.PTPMOVJANGLEMode, -12.281300, -42.151200, -65.722000, 107.873000, 102.281000, -0.000000, isQueued=1)
    dType.SetPTPCmd(api,dobotId, dType.PTPMode.PTPMOVJANGLEMode, -12.281300, -42.151200, -65.722000, 107.873000, 102.281000, -47.500000, isQueued=1)
    dType.SetPTPCmd(api,dobotId, dType.PTPMode.PTPMOVLANGLEMode, 0.000000, 0.000000, -90.000000, 90.000000, 90.000000, -0.000000, isQueued=1)
    print('Program Prog1 sent')
    sys.stdout.flush()
    print('Running program Prog1 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 Prog1 Finished')
    sys.stdout.flush()


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

#Disconnect Dobot
dType.DisconnectDobot(api,dobotId)

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