import robot_control
import dobot
import serial
import time
import math

dobot.setQueuedCmdClear()
dobot.setQueuedCmdStartExec()
dobot.setHomeCmd()
dobot.setCPParams(175, 0, 10, 1, 1)

delay = 1
count = 0
for i in range(4):
    x = 10*i + 100
    y = 10*i + 50
    z = 10*i + 50
    count += 1
    r = math.atan2(x, y)
    dobot.setCPCmd(1, x, y, z, r, 1)
    time.sleep(delay)