"""
Урок 11 (проект): робот выполняет ПРОГРАММУ — переставляет предмет.

Программа робота — это список поз. Каждая поза = [база, плечо, локоть, захват].
Робот проходит позы по очереди с паузами: подъехать, открыть, опуститься,
закрыть (схватить), поднять, повернуть, опустить, открыть (отпустить).

Запусти:  python demo_pick.py
Углы — примерные! Подбери их под свою сборку в уроках 9–10 и впиши сюда.
"""
import time
from arm import connect

arm = connect()

# [база, плечо, локоть, захват]   (захват: больше = более открыт)
PROGRAM = [
    [90, 90, 90, 90],     # 1. исходная, захват открыт
    [40, 90, 90, 90],     # 2. повернулись к предмету
    [40, 60, 60, 90],     # 3. опустились к предмету
    [40, 60, 60, 30],     # 4. сжали захват — взяли
    [40, 110, 110, 30],   # 5. подняли
    [140, 110, 110, 30],  # 6. перенесли в другую сторону
    [140, 60, 60, 30],    # 7. опустили
    [140, 60, 60, 90],    # 8. открыли захват — отпустили
    [90, 110, 110, 90],   # 9. вернулись домой
]
PAUSE = 1.0               # секунд между позами (дать роботу доехать)


def play(program):
    for step, pose in enumerate(program, start=1):
        print(f"Шаг {step}: {pose}")
        for joint in range(4):
            arm.set(joint, pose[joint])
        time.sleep(PAUSE)


if __name__ == "__main__":
    print("▶ Запускаю программу робота…")
    play(PROGRAM)
    print("✅ Готово!")
    arm.close()