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

# xmin = 128, max = 267, ymin 180 -180
dobot.setQueuedCmdClear()
dobot.setQueuedCmdStartExec()
dobot.setHomeCmd()


dobot.setCPParams(175,0,10,1,1)

for i in range(3):
    dobot.setCPCmd(1, 200, 0, 50, 0, 1)
    dobot.setCPCmd(1, 250, 0, -50, 0, 1)
    dobot.setCPCmd(1, 180, 0, -50, 0, 1)
    print(dobot.getPose())
    time.sleep(2)