import coords_creation
import robot_control
import dobot
import serial
import time
import math


y = []
x = []


coords = []

origin = [200, 0, -63]      # origin of the drawing
new_size = [60, 0]         # new size of the picture in mm, write 0 if you want to be proportional
angle = 90                 # rotation of 180°
sym = 'x'                   # symetry on the x-axis
live = 0                    # 0 is live, 1 is for saved image

coords = coords_creation.coordinates(origin, new_size, angle, sym, live)

# xmin = 128, max = 267, ymin 180 -180

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

delay = 0.3
count = 0
for i in coords:
    x = i[0]
    y = i[1]
    z = i[2]
    count += 1
    r = math.atan2(x, y)
    dobot.setCPCmd(1, x, y, z, r, 1)
    time.sleep(delay)
    print(round(count/len(coords)*100, 4), "%")