import matplotlib.pyplot as plt
import sys
#sys.path.insert(0, './dobot/librairies')
import dobot
import time
import math

dobot.setQueuedCmdClear()
dobot.setQueuedCmdStartExec()

dobot.setPTPjointParams(200,200,200,200,200,200,200,200,1)

dobot.setPTPCmd(1, 250, 0, 150, 0, 1)

dobot.setPTPjointParams(200,1400,1400,200,200,1400,1400,200,1)

dobot.setPTPCmd(1, 250, 0, -90, 0, 1)

dobot.setPTPjointParams(200,200,200,200,200,200,200,200,1)

dobot.setPTPCmd(1, 250, 0, 150, 0, 1)

"""
dobot.setPTPCmd(1, -181, -272, 12, 0, 1)

dobot.setEndEffectorSuctionCup(1,1)

dobot.setWaitCmd(3000,1)

dobot.setPTPjointParams(2000,200,200,200,1100,200,200,200,1)
print(dobot.getPTPjointParams())

dobot.setPTPCmd(6, 245, 0, 0, 0, 1)

dobot.setPTPjointParams(400,200,200,200,400,200,200,200,1)

dobot.setPTPCmd(1, -181, -272, 12, 0, 1)

time.sleep(3.7)

dobot.setEndEffectorSuctionCup(0,0)
"""