diff --git a/drkota-motor b/drkota-motor new file mode 100755 index 0000000..155bf68 --- /dev/null +++ b/drkota-motor @@ -0,0 +1,337 @@ +#!/usr/bin/env python3 +""" +drkota-motor: CLI nástroj pro kalibraci a rozhýbání krokových motorů V-plotteru (Drkota). +Komunikuje s Arduino CNC Shieldem přes /dev/ttyUSB0 (lokálně nebo transparentně přes SSH na t520). +""" + +import sys +import os +import time +import json +import subprocess +import argparse + +BAUD_RATE = 115200 +DEFAULT_PORT = "/dev/ttyUSB0" +STATE_FILE = "/tmp/drkota_pos.json" + +def get_target_host(): + """Vrátí 'local', pokud ttyUSB0 existuje lokálně, jinak 't520'.""" + if os.path.exists(DEFAULT_PORT): + return "local" + # Zkontroluj, zda ttyUSB0 existuje na t520 + try: + res = subprocess.run(["ssh", "-q", "t520", "test -e " + DEFAULT_PORT], timeout=3) + if res.returncode == 0: + return "t520" + except Exception: + pass + return "local" + +def run_remote_if_needed(): + target = get_target_host() + if target == "t520" and os.uname().nodename != "t520": + # Spusť tento skript na t520 se stejnými argumenty + cmd = ["ssh", "-t", "t520", "python3 - " + " ".join([f'"{a}"' for a in sys.argv[1:]])] + # Pošli obsah tohoto skriptu na stdin + with open(__file__, "rb") as f: + code = f.read() + p = subprocess.Popen(cmd, stdin=subprocess.PIPE) + p.communicate(input=code) + sys.exit(p.returncode) + +run_remote_if_needed() + +# --- Zde již běžíme přímo na stroji s připojeným hardwarem --- +import serial + +def load_pos(): + if os.path.exists(STATE_FILE): + try: + with open(STATE_FILE, "r") as f: + d = json.load(f) + return d.get("x", 0), d.get("y", 0) + except Exception: + pass + return 0, 0 + +def save_pos(x, y): + try: + with open(STATE_FILE, "w") as f: + json.dump({"x": x, "y": y}, f) + except Exception: + pass + +def open_serial(port=DEFAULT_PORT, timeout=5): + if not os.path.exists(port): + print(f"[CHYBA] Sériový port {port} neexistuje!") + sys.exit(1) + s = serial.Serial(port, BAUD_RATE, timeout=timeout) + time.sleep(1.8) # čekání na bootloader / DTR reset + s.reset_input_buffer() + return s + +def send_cmd(s, cmd): + clean = cmd.strip() + s.write((clean + "\n").encode("utf-8")) + resp = s.readline().decode("utf-8", errors="ignore").strip() + return resp + +def cmd_status(args): + print(f"=== Drkota Motor Status ===") + print(f"Host: {os.uname().nodename}") + print(f"Port: {args.port} @ {BAUD_RATE} baud") + cur_x, cur_y = load_pos() + print(f"Uložená pozice: X={cur_x}, Y={cur_y} kroků") + + try: + s = open_serial(args.port, timeout=2) + resp = send_cmd(s, "P0") + s.close() + if resp == "ok": + print("Odezva Arduina: [OK] (Spojení je aktivní a funkční)") + else: + print(f"Odezva Arduina: [{resp}] (Neočekávaná odezva)") + except Exception as e: + print(f"Chyba spojení: {e}") + +def cmd_zero(args): + """Resetuje Arduino a nastaví software počátek na 0,0.""" + print("Resetuji software pozici a Arduino...") + try: + s = serial.Serial(args.port, BAUD_RATE, timeout=1) + s.setDTR(False) + time.sleep(0.1) + s.setDTR(True) + time.sleep(1.5) + s.close() + save_pos(0, 0) + print("Pozice resetována na X=0, Y=0. Motory jsou v nulovém bodě.") + except Exception as e: + print(f"Chyba při resetu: {e}") + +def cmd_jog(args): + """Posune motory o relativní počet kroků.""" + dx = args.dx + dy = args.dy + cur_x, cur_y = load_pos() + target_x = cur_x + dx + target_y = cur_y + dy + + print(f"Posun o rel: dX={dx:+d}, dY={dy:+d} -> Cíl: X={target_x}, Y={target_y}") + s = open_serial(args.port, timeout=10) + cmd = f"X{target_x}Y{target_y}" + print(f"Odesílám: {cmd}") + resp = send_cmd(s, cmd) + s.close() + + if resp == "ok": + save_pos(target_x, target_y) + print("Pohyb dokončen: [OK]") + else: + print(f"Chyba odezvy: {resp}") + +def cmd_spin(args): + """Roztočí zvolený motor o daný počet kroků.""" + motor = args.motor.lower() + steps = args.steps + dx, dy = 0, 0 + if motor in ("x", "left", "l", "1"): + dx = steps + elif motor in ("y", "right", "r", "2"): + dy = steps + elif motor in ("both", "xy", "all"): + dx = steps + dy = steps + else: + print(f"Neznámý motor: {motor}. Použij 'x', 'y' nebo 'both'.") + return + + cur_x, cur_y = load_pos() + target_x = cur_x + dx + target_y = cur_y + dy + + print(f"Spin motor {motor.upper()} o {steps:+d} kroků...") + s = open_serial(args.port, timeout=15) + resp = send_cmd(s, f"X{target_x}Y{target_y}") + s.close() + if resp == "ok": + save_pos(target_x, target_y) + print(f"Spin dokončen: [OK]") + else: + print(f"Chyba odezvy: {resp}") + +def cmd_test(args): + """Otestuje oba motory pohybem tam a zpět (800 kroků).""" + steps = args.steps + print(f"=== Spouštím diagnostický test motorů ({steps} kroků) ===") + print("1. Krok: Motor X (levý) dopředu...") + s = open_serial(args.port, timeout=10) + + cur_x, cur_y = load_pos() + + # 1. X forward + send_cmd(s, f"X{cur_x + steps}Y{cur_y}") + time.sleep(0.3) + + # 2. X backward + print("2. Krok: Motor X (levý) zpět...") + send_cmd(s, f"X{cur_x}Y{cur_y}") + time.sleep(0.3) + + # 3. Y forward + print("3. Krok: Motor Y (pravý) dopředu...") + send_cmd(s, f"X{cur_x}Y{cur_y + steps}") + time.sleep(0.3) + + # 4. Y backward + print("4. Krok: Motor Y (pravý) zpět...") + send_cmd(s, f"X{cur_x}Y{cur_y}") + time.sleep(0.3) + + # 5. Both forward + print("5. Krok: Oba motory současně dopředu a zpět...") + send_cmd(s, f"X{cur_x + steps}Y{cur_y + steps}") + time.sleep(0.3) + send_cmd(s, f"X{cur_x}Y{cur_y}") + + s.close() + print("=== Diagnostický test motorů úspěšně dokončen! ===") + +def cmd_calib(args): + """Vypočítá kroky na milimetr na základě fyzického měření.""" + steps = args.steps + measured_mm = args.measured_mm + if measured_mm <= 0: + print("[CHYBA] Naměřené milimetry musí být větší než 0!") + return + + steps_per_mm = steps / measured_mm + print(f"\n=== Výpočet kalibrace StepsPerMm ===") + print(f"Zadaný posun: {steps} kroků") + print(f"Skutečně naměřeno posuvným měřítkem / metrem: {measured_mm:.2f} mm") + print(f"-> Skutečná hodnota: StepsPerMm = {steps_per_mm:.4f} kroků/mm\n") + + # Porovnání se standardními řemenicemi GT2 (20 zubů = 40 mm/otáčka) + rev_mm = 40.0 + steps_per_rev = steps_per_mm * rev_mm + print(f"Při GT2 řemenici (40 mm/ot) to odpovídá cca {steps_per_rev:.0f} krokům na otáčku:") + if abs(steps_per_rev - 200) < 30: + print(" -> Pravděpodobně ZÁKLADNÍ KROK (Full-step, bez mikrokrokování, cca 5 kroků/mm)") + elif abs(steps_per_rev - 400) < 50: + print(" -> Pravděpodobně 1/2 mikrokrok (cca 10 kroků/mm)") + elif abs(steps_per_rev - 800) < 100: + print(" -> Pravděpodobně 1/4 mikrokrok (cca 20 kroků/mm)") + elif abs(steps_per_rev - 1600) < 200: + print(" -> Pravděpodobně 1/8 mikrokrok (cca 40 kroků/mm)") + elif abs(steps_per_rev - 3200) < 300: + print(" -> Pravděpodobně 1/16 mikrokrok (cca 80 kroků/mm)") + else: + print(f" -> Vlastní převod / cívka.") + +def cmd_interactive(args): + """Interaktivní ovládání z klávesnice.""" + import tty, termios + + print("=== Interaktivní režim motorů ===") + print("Ovládání:") + print(" [A] / [D] : Motor X (levý) - / +") + print(" [W] / [S] : Motor Y (pravý) + / -") + print(" [J] / [L] : Oba motory - / +") + print(" [1] 10 kroků | [2] 50 kroků | [3] 200 kroků (1 ot) | [4] 800 kroků") + print(" [Z] Vynulovat pozici") + print(" [Q] Konec") + + step_size = 200 + s = open_serial(args.port, timeout=5) + cur_x, cur_y = load_pos() + + fd = sys.stdin.fileno() + old_settings = termios.tcgetattr(fd) + try: + tty.setraw(fd) + while True: + ch = sys.stdin.read(1) + if ch.lower() == 'q' or ch == '\x03': # Ctrl+C or Q + break + elif ch == '1': + step_size = 10 + elif ch == '2': + step_size = 50 + elif ch == '3': + step_size = 200 + elif ch == '4': + step_size = 800 + elif ch == 'z' or ch == 'Z': + cur_x, cur_y = 0, 0 + save_pos(0, 0) + else: + dx, dy = 0, 0 + if ch in ('a', 'A'): + dx = -step_size + elif ch in ('d', 'D'): + dx = step_size + elif ch in ('w', 'W'): + dy = step_size + elif ch in ('s', 'S'): + dy = -step_size + elif ch in ('j', 'J'): + dx = -step_size + dy = -step_size + elif ch in ('l', 'L'): + dx = step_size + dy = step_size + + if dx != 0 or dy != 0: + cur_x += dx + cur_y += dy + send_cmd(s, f"X{cur_x}Y{cur_y}") + save_pos(cur_x, cur_y) + finally: + termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) + s.close() + print(f"\nUkončeno. Aktuální pozice: X={cur_x}, Y={cur_y}") + +def main(): + parser = argparse.ArgumentParser(description="Drkota Motor CLI Tool") + parser.add_argument("--port", default=DEFAULT_PORT, help="Sériový port (výchozí /dev/ttyUSB0)") + sub = parser.add_subparsers(dest="subcommand") + + sub.add_parser("status", help="Ověřit stav a spojení") + sub.add_parser("zero", help="Vynulovat pozici") + + p_jog = sub.add_parser("jog", help="Relativní posun dX dY kroků") + p_jog.add_argument("dx", type=int, help="Kroky X") + p_jog.add_argument("dy", type=int, help="Kroky Y") + + p_spin = sub.add_parser("spin", help="Roztočit motor o N kroků") + p_spin.add_argument("motor", choices=["x", "y", "both"], help="Který motor") + p_spin.add_argument("steps", type=int, help="Počet kroků") + + p_test = sub.add_parser("test", help="Diagnostický test motorů") + p_test.add_argument("--steps", type=int, default=800, help="Počet kroků pro test") + + p_calib = sub.add_parser("calib", help="Výpočet StepsPerMm z měření") + p_calib.add_argument("steps", type=float, help="Zadané kroky") + p_calib.add_argument("measured_mm", type=float, help="Naměřená vzdálenost v mm") + + sub.add_parser("interactive", help="Interaktivní řízení z klávesnice") + + args = parser.parse_args() + if not args.subcommand: + parser.print_help() + return + + cmds = { + "status": cmd_status, + "zero": cmd_zero, + "jog": cmd_jog, + "spin": cmd_spin, + "test": cmd_test, + "calib": cmd_calib, + "interactive": cmd_interactive, + } + cmds[args.subcommand](args) + +if __name__ == "__main__": + main()