from robolink import *  # Подключаем RoboDK
from robodk import *  # Подключаем RoboDK функции
import math

# Подключение к RoboDK
RDK = Robolink()

# Получаем робота по его имени
robot = RDK.Item('KUKA KR 90 R3100 extra')
extruder = RDK.Item('Extruder_TCP')

# Устанавливаем ориентацию экструдера, ось Z направлена вниз
extruder.setPose(Pose(0, 0, 0, 90, 0, 0))

# Получаем текущую позицию робота
current_position = robot.Pose()
print("Текущая позиция робота:", current_position)

# Печатаем безопасные координаты
x_start = 500  # Начальная координата по X
y_start = 0  # Начальная координата по Y
z_start = 100  # Начальная координата по Z

# Устанавливаем начальную позицию
start_position = Pose(x_start, y_start, z_start, 0, 0, 0)

# Перемещаем робота в безопасную начальную позицию
robot.MoveJ(start_position)

# Параметры вазы
height = 700  # Высота вазы
radius = 200  # Радиус вазы
num_layers = 700  # Количество слоев (чем больше, тем мельче шаг)
layer_height = height / num_layers  # Высота одного слоя

# Печать вазы (спираль)
for layer in range(num_layers):
    # Вычисляем угол для текущего слоя (спиральное движение)
    angle = 2 * math.pi * layer / num_layers  # 360 градусов за весь цикл
    
    # Рассчитываем координаты X, Y для текущего слоя
    x = x_start + radius * math.cos(angle)
    y = y_start + radius * math.sin(angle)
    z = z_start + layer * layer_height  # Поднимаем экструдер на слой
    
    # Перемещаем робота на новую точку
    robot.MoveL(Pose(x, y, z, 0, 0, 0))

print("Печать вазы завершена.")