BlueGit

timfa timdraw

Files

timdraw / timdraw / grbl.py

grbl.py

grbl.py 24.4 KiB Download Raw
1"""Seriële GRBL 1.1-verbinding voor de Bachin TA4 (CH340, 115200)."""
2
3from __future__ import annotations
4
5import glob
6import json
7import math
8import os
9import re
10import threading
11import time
12from pathlib import Path
13
14BED_W = 200.0
15BED_H = 297.0
16
17try:
18 import serial
19 from serial.serialutil import SerialException
20except ImportError: # pragma: no cover
21 serial = None
22 SerialException = OSError
23
24
25_STATE = Path.home() / ".config" / "timdraw" / "last-device.json"
26
27
28def chord_for_arc(line: str) -> str:
29 """Rechte lijn naar hetzelfde eindpunt, als GRBL een boog met error 33 weigert."""
30 words = dict(re.findall(r"([XYF])([+-]?(?:\d+\.\d*|\.\d+|\d+))", line))
31 if "X" not in words or "Y" not in words:
32 raise RuntimeError("error:33")
33 chord = f"G1 X{words['X']} Y{words['Y']}"
34 if "F" in words:
35 chord += f" F{words['F']}"
36 return chord
37
38
39def _mm(value: float) -> str:
40 rounded = round(value)
41 if abs(value - rounded) < 0.05:
42 return str(int(rounded))
43 return f"{value:.1f}"
44
45
46def limit_step(
47 x: float, y: float, dx: float, dy: float, origin_top: bool = True
48) -> tuple[float, float, str | None]:
49 """Houd de stap op het blad. Linksboven: X 0..200, Y -297..0."""
50 y_min, y_max = (-BED_H, 0.0) if origin_top else (0.0, BED_H)
51 nx = min(BED_W, max(0.0, x + dx))
52 ny = min(y_max, max(y_min, y + dy))
53 adx = nx - x
54 ady = ny - y
55 requested = math.hypot(dx, dy)
56 actual = math.hypot(adx, ady)
57 if requested > 0.05 and actual + 0.05 < requested:
58 return adx, ady, f"{_mm(actual)} van {_mm(requested)} mm bewogen, daarna limiet bereikt"
59 return adx, ady, None
60
61
62def resolve_work_position(
63 mpos: list[float] | None,
64 wpos: list[float] | None,
65 wco: list[float] | None,
66 remembered_wco: list[float] | None,
67) -> tuple[list[float] | None, list[float] | None]:
68 """Werkpositie uit een statusregel. GRBL stuurt WCO maar af en toe mee."""
69 if wco is not None:
70 remembered_wco = wco
71 if wpos is not None:
72 return wpos, remembered_wco
73 if mpos is not None and remembered_wco is not None:
74 return [mpos[i] - remembered_wco[i] for i in range(3)], remembered_wco
75 return None, remembered_wco
76
77
78def list_ports() -> list[str]:
79 by_id = sorted(glob.glob("/dev/serial/by-id/*"))
80 if by_id:
81 return by_id
82 return sorted(glob.glob("/dev/ttyUSB*") + glob.glob("/dev/ttyACM*"))
83
84
85def load_last_port() -> str:
86 try:
87 port = json.loads(_STATE.read_text(encoding="utf-8")).get("port") or ""
88 except (OSError, json.JSONDecodeError):
89 return ""
90 return str(port)
91
92
93def save_last_port(port: str) -> None:
94 _STATE.parent.mkdir(parents=True, exist_ok=True)
95 _STATE.write_text(json.dumps({"port": port}), encoding="utf-8")
96
97
98def resolve_port(preferred: str | None) -> str:
99 ports = list_ports()
100 if preferred and preferred in ports:
101 return preferred
102 if preferred:
103 try:
104 target = os.path.realpath(preferred)
105 except OSError:
106 target = ""
107 for port in ports:
108 try:
109 if os.path.realpath(port) == target:
110 return port
111 except OSError:
112 continue
113 if preferred and os.path.exists(preferred):
114 return preferred
115 return ports[0] if len(ports) == 1 and not preferred else ""
116
117
118class Machine:
119 def __init__(self) -> None:
120 self._io = threading.Lock()
121 self._state_lock = threading.Lock()
122 self._ser = None
123 self._reader: threading.Thread | None = None
124 self._job: threading.Thread | None = None
125 self._acks: list[str] = []
126 self._ack_event = threading.Event()
127 self._stop = threading.Event()
128 self._pause = threading.Event()
129 self._arc_fallbacks = 0
130 self.connected = False
131 self.connecting = False
132 self.auto_enabled = True
133 self.last_port = load_last_port()
134 self.port = ""
135 self._connect_lock = threading.Lock()
136 self._watch_stop = threading.Event()
137 self._watcher: threading.Thread | None = None
138 self.firmware = ""
139 self.state = "disconnected"
140 self.wpos = [0.0, 0.0, 0.0]
141 self.mpos = [0.0, 0.0, 0.0]
142 self._wco: list[float] | None = None
143 self._track_xy: tuple[float, float] | None = None
144 self._track_until = 0.0
145 self.message = "Niet verbonden"
146 self.error = ""
147 self.progress = 0.0
148 self.job_active = False
149 self.job_paused = False
150 self.max_rate = 6000.0
151 self._rx = ""
152
153 def status(self) -> dict:
154 if self.connected and self._ser is not None:
155 self._query()
156 with self._state_lock:
157 return {
158 "connected": self.connected,
159 "connecting": self.connecting,
160 "port": self.port,
161 "lastPort": self.last_port,
162 "localUrl": getattr(self, "local_url", ""),
163 "ports": list_ports(),
164 "firmware": self.firmware,
165 "state": self.state,
166 "wpos": list(self.wpos),
167 "mpos": list(self.mpos),
168 "message": self.message,
169 "error": self.error,
170 "progress": self.progress if self.job_active else None,
171 "jobActive": self.job_active,
172 "jobPaused": self.job_paused,
173 "maxRate": self.max_rate,
174 }
175
176 def start_auto(self) -> None:
177 if self._watcher and self._watcher.is_alive():
178 return
179 self._watch_stop.clear()
180 self._watcher = threading.Thread(target=self._watch_ports, daemon=True)
181 self._watcher.start()
182
183 def stop_auto(self) -> None:
184 self.auto_enabled = False
185 self._watch_stop.set()
186
187 def _watch_ports(self) -> None:
188 absent = True
189 while not self._watch_stop.wait(1.2):
190 if not self.auto_enabled or self.connected or self.connecting:
191 continue
192 if not self.last_port:
193 continue
194 port = resolve_port(self.last_port)
195 if not port:
196 absent = True
197 continue
198 if not absent:
199 continue
200 absent = False
201 try:
202 self.connect(port)
203 except Exception as exc:
204 self.connecting = False
205 self.error = str(exc)
206 self.message = str(exc)
207
208 def connect(self, port: str | None = None) -> None:
209 with self._connect_lock:
210 self._connect(port)
211
212 def _connect(self, port: str | None) -> None:
213 if serial is None:
214 raise RuntimeError("pyserial ontbreekt. Installeer het met: pip install pyserial")
215 port = resolve_port(port or self.last_port or None)
216 if (
217 self.connected
218 and self.port
219 and port
220 and os.path.realpath(self.port) == os.path.realpath(port)
221 ):
222 return
223 self.disconnect()
224 if not port:
225 raise RuntimeError("Geen seriële poort gevonden. Zit de USB-kabel erin?")
226 self.connecting = True
227 try:
228 self._open(port)
229 finally:
230 self.connecting = False
231
232 def _open(self, port: str) -> None:
233 try:
234 ser = serial.Serial()
235 ser.port = port
236 ser.baudrate = 115200
237 ser.timeout = 0.2
238 ser.dtr = False
239 ser.rts = False
240 ser.open()
241 except SerialException as exc:
242 text = str(exc)
243 if "Permission" in text or "toegang" in text.lower() or "denied" in text.lower():
244 raise RuntimeError(
245 f"Geen toegang tot {port}. De poort hoort bij groep uucp. "
246 "Eenmalig in een terminal: sudo usermod -aG uucp $USER "
247 "en daarna uitloggen en opnieuw inloggen."
248 ) from exc
249 raise RuntimeError(f"Kan {port} niet openen: {exc}") from exc
250 self._ser = ser
251 self.port = port
252 self.last_port = port
253 self.connected = True
254 self.auto_enabled = True
255 self.connecting = False
256 self.error = ""
257 self.message = "Verbinden…"
258 try:
259 save_last_port(port)
260 except OSError:
261 pass
262 self._stop.clear()
263 self._reader = threading.Thread(target=self._read_loop, daemon=True)
264 self._reader.start()
265 time.sleep(0.3)
266 self._write_raw(b"\n\n")
267 time.sleep(0.4)
268 self._query()
269 try:
270 self._command("$I", timeout=2)
271 self._command("$$", timeout=2)
272 except Exception as exc:
273 self.message = f"Verbonden, instellingen niet gelezen: {exc}"
274 try:
275 self._hold_motors()
276 except Exception as exc:
277 self.message = f"Verbonden, motoren niet vastgezet: {exc}"
278
279 def _hold_motors(self) -> None:
280 # $1=255 houdt de stappenmotoren bekrachtigd. Bij 254 vallen ze na
281 # een kwart seconde uit en trekt de veer de pen meteen weer omhoog.
282 reply = self._command("$1=255", timeout=3)
283 if reply.startswith("error"):
284 raise RuntimeError(reply)
285
286 def disconnect(self, manual: bool = False) -> None:
287 if manual:
288 self.auto_enabled = False
289 self.stop_job()
290 self.connected = False
291 self.state = "disconnected"
292 self.message = "Niet verbonden"
293 reader = self._reader
294 ser = self._ser
295 self._ser = None
296 self._reader = None
297 if ser is not None:
298 try:
299 ser.close()
300 except Exception:
301 pass
302 if reader is not None and reader.is_alive() and threading.current_thread() is not reader:
303 reader.join(timeout=1)
304
305 def attach(self, port) -> None:
306 """Koppel een al open poort. De testsuite gebruikt dit voor de virtuele machine."""
307 self.disconnect()
308 self._ser = port
309 self.port = "virtual"
310 self.connected = True
311 self.auto_enabled = False
312 self.error = ""
313 self.message = "Verbinden…"
314 self._stop.clear()
315 self._reader = threading.Thread(target=self._read_loop, daemon=True)
316 self._reader.start()
317 self._command("$I", timeout=2)
318 self._command("$$", timeout=2)
319 self._hold_motors()
320 self._query()
321 time.sleep(0.05)
322
323 def _write_raw(self, data: bytes) -> None:
324 with self._io:
325 if self._ser is not None:
326 self._ser.write(data)
327
328 def _query(self) -> None:
329 if self.connected:
330 self._write_raw(b"?")
331
332 def _read_loop(self) -> None:
333 while self._ser is not None and self.connected:
334 try:
335 chunk = self._ser.read(256)
336 except Exception:
337 break
338 if not chunk:
339 continue
340 self._rx += chunk.decode("utf-8", "replace")
341 while "\n" in self._rx:
342 line, self._rx = self._rx.split("\n", 1)
343 self._handle(line.strip("\r"))
344 self.connected = False
345
346 def _handle(self, line: str) -> None:
347 if not line:
348 return
349 if line.startswith("<") and line.endswith(">"):
350 self._parse_status(line[1:-1])
351 return
352 if line == "ok" or line.startswith("error:"):
353 with self._state_lock:
354 self._acks.append(line)
355 self._ack_event.set()
356 if line.startswith("error:"):
357 self.error = line
358 return
359 if line.startswith("Grbl"):
360 with self._state_lock:
361 self.firmware = line
362 self.message = line
363 return
364 if line.startswith("ALARM") or line.startswith("ALARM:"):
365 with self._state_lock:
366 self.state = "Alarm"
367 self.error = line
368 return
369 if line.startswith("$") and "=" in line:
370 self._parse_setting(line)
371 return
372 if line.startswith("[VER:") or line.startswith("[OPT:"):
373 with self._state_lock:
374 self.firmware = (self.firmware + " " + line).strip()
375
376 def _parse_setting(self, line: str) -> None:
377 try:
378 key, value = line.split("=", 1)
379 number = float(value)
380 except ValueError:
381 return
382 if key in ("$110", "$111"):
383 self.max_rate = min(self.max_rate, number) if self.max_rate else number
384 if key == "$110":
385 self.max_rate = number
386
387 def _parse_status(self, body: str) -> None:
388 parts = body.split("|")
389 state = parts[0]
390 mpos = None
391 wpos = None
392 wco = None
393 for part in parts[1:]:
394 if part.startswith("MPos:"):
395 mpos = [float(v) for v in part[5:].split(",")[:3]]
396 elif part.startswith("WPos:"):
397 wpos = [float(v) for v in part[5:].split(",")[:3]]
398 elif part.startswith("WCO:"):
399 wco = [float(v) for v in part[4:].split(",")[:3]]
400 with self._state_lock:
401 self.state = state
402 if mpos:
403 self.mpos = mpos
404 resolved, self._wco = resolve_work_position(mpos, wpos, wco, self._wco)
405 if resolved:
406 self.wpos = resolved
407 settled = state.lower().startswith("idle") and not self.job_active
408 if settled and (time.time() >= self._track_until or self._track_xy is None or (
409 abs(resolved[0] - self._track_xy[0]) < 0.4 and abs(resolved[1] - self._track_xy[1]) < 0.4
410 )):
411 self._track_xy = (resolved[0], resolved[1])
412 if state.lower().startswith("idle") and not self.job_active and not self.message.startswith("Klaar"):
413 self.message = "Gereed"
414
415 def _command(self, line: str, timeout: float = 15) -> str:
416 if not self.connected or self._ser is None:
417 raise RuntimeError("De machine is niet verbonden.")
418 with self._state_lock:
419 self._acks.clear()
420 self._ack_event.clear()
421 self._write_raw((line.strip() + "\n").encode())
422 deadline = time.time() + timeout
423 while time.time() < deadline:
424 with self._state_lock:
425 if self._acks:
426 return self._acks.pop(0)
427 self._ack_event.wait(0.2)
428 self._ack_event.clear()
429 raise TimeoutError(f"Geen antwoord op {line}")
430
431 def jog(self, dx: float, dy: float, feed: float, origin_top: bool = True) -> str | None:
432 dx = max(-80, min(80, dx))
433 dy = max(-80, min(80, dy))
434 feed = max(50, min(self.max_rate or 6000, feed))
435 if abs(dx) < 1e-6 and abs(dy) < 1e-6:
436 return None
437 self._hold_motors()
438 x, y = self._track_xy if self._track_xy is not None else (self.wpos[0], self.wpos[1])
439 dx, dy, notice = limit_step(x, y, dx, dy, origin_top)
440 if abs(dx) < 1e-6 and abs(dy) < 1e-6:
441 return notice
442 parts = ["$J=G90 G21"]
443 target_x, target_y = x + dx, y + dy
444 parts.append(f"X{target_x:.3f}")
445 parts.append(f"Y{target_y:.3f}")
446 parts.append(f"F{feed:.0f}")
447 reply = self._command(" ".join(parts), timeout=8)
448 self._query()
449 if reply.startswith("error"):
450 raise RuntimeError(reply + " — staat de machine in alarm? Ontgrendel of home eerst.")
451 self._track_xy = (target_x, target_y)
452 self._track_until = time.time() + max(0.4, math.hypot(dx, dy) / feed * 60.0 + 0.6)
453 return notice
454
455 def pen(self, down: bool, up_z: float = 0, down_z: float = 5, feed: float = 5000) -> None:
456 z = down_z if down else up_z
457 z = max(0, min(12, z))
458 feed = max(50, min(self.max_rate or 6000, feed))
459 self._hold_motors()
460 self._command("G21")
461 self._command("G90")
462 reply = self._command(f"G1 Z{z:.3f} F{feed:.0f}", timeout=20)
463 self._query()
464 if reply.startswith("error"):
465 raise RuntimeError(reply)
466
467 def _sync(self, line: str, timeout: float, job: bool) -> None:
468 if job:
469 self._job_line(line, timeout)
470 return
471 reply = self._command(line, timeout=timeout)
472 if reply.startswith("error"):
473 raise RuntimeError(reply)
474
475 def _loosen(self, job: bool) -> None:
476 self._sync("G21", 5, job)
477 self._sync("G91", 5, job)
478 self._sync("$1=0", 5, job)
479 self._sync("G1 X0.050 F500", 15, job)
480 self._wait_until_idle(20)
481 self._sync("G90", 5, job)
482
483 def release_axes(self) -> None:
484 self._stop.clear()
485 try:
486 self._loosen(False)
487 except Exception:
488 try:
489 self._sync("G90", 5, False)
490 except Exception:
491 pass
492 raise
493 self._query()
494
495 def _prepare_home(self, job: bool) -> None:
496 """Laat de assen los zodat de veer de pen optilt, wacht een seconde, en home dan."""
497 try:
498 self._loosen(job)
499 deadline = time.time() + 1.0
500 while time.time() < deadline:
501 if job and self._stop.is_set():
502 break
503 time.sleep(0.05)
504 self._sync("$1=255", 5, job)
505 if job and self._stop.is_set():
506 return
507 self._sync("$H", 120, job)
508 self._sync("G92 X0 Y0", 5, job)
509 self.wpos[0] = 0.0
510 self.wpos[1] = 0.0
511 self._track_xy = (0.0, 0.0)
512 self._track_until = 0.0
513 if len(self.mpos) >= 2:
514 if self._wco is None:
515 self._wco = [0.0, 0.0, 0.0]
516 self._wco[0] = self.mpos[0]
517 self._wco[1] = self.mpos[1]
518 except Exception:
519 try:
520 self._sync("G90", 5, job)
521 self._sync("$1=255", 5, job)
522 except Exception:
523 pass
524 raise
525
526 def home(self) -> None:
527 self._stop.clear()
528 self._prepare_home(False)
529 self._query()
530
531 def unlock(self) -> None:
532 reply = self._command("$X", timeout=5)
533 self._query()
534 if reply.startswith("error"):
535 raise RuntimeError(reply)
536
537 def start_job(self, gcode: str) -> None:
538 if self.job_active:
539 raise RuntimeError("Er loopt al een tekening.")
540 lines = []
541 for raw in gcode.splitlines():
542 line = raw.split(";", 1)[0].strip()
543 if line:
544 lines.append(line)
545 if not lines:
546 raise RuntimeError("Er is niets om te tekenen.")
547 self._pause.clear()
548 self._stop.clear()
549 with self._state_lock:
550 self._acks.clear()
551 self._ack_event.clear()
552 self.job_active = True
553 self.job_paused = False
554 self.progress = 0
555 self.error = ""
556 self._arc_fallbacks = 0
557 self.message = "Tekenen…"
558 self._job = threading.Thread(target=self._run_job, args=(lines,), daemon=True)
559 self._job.start()
560
561 def pause_job(self) -> None:
562 if not self.job_active:
563 return
564 self._pause.set()
565 self.job_paused = True
566 self._write_raw(b"!")
567 self.message = "Pauze"
568
569 def resume_job(self) -> None:
570 if not self.job_active:
571 return
572 self._pause.clear()
573 self.job_paused = False
574 self._write_raw(b"~")
575 self.message = "Tekenen…"
576
577 def stop_job(self) -> None:
578 self._stop.set()
579 self._pause.clear()
580 if self._ser is not None and (self.job_active or self.state.lower().startswith("run") or self.state.lower().startswith("hold")):
581 self._write_raw(b"!")
582 time.sleep(0.15)
583 self._write_raw(b"\x18")
584 self.job_active = False
585 self.job_paused = False
586 self.progress = 0
587 if self.connected:
588 self.message = "Gestopt"
589
590 def _run_job(self, lines: list[str]) -> None:
591 used = 0
592 pending: list[int] = []
593 sent = 0
594 try:
595 if not self._stop.is_set():
596 self.message = "Homen…"
597 self._prepare_home(True)
598 for line in lines:
599 while self._pause.is_set() and not self._stop.is_set():
600 time.sleep(0.05)
601 if self._stop.is_set():
602 break
603 if line.startswith("$") or line.startswith(("G2 ", "G3 ")):
604 while pending and not self._stop.is_set():
605 ack = self._wait_ack(60)
606 used -= pending.pop(0)
607 if ack.startswith("error"):
608 raise RuntimeError(ack)
609 if line.startswith("$"):
610 self._wait_until_idle(60)
611 if self._stop.is_set():
612 break
613 self._job_line(line, 30 if line.startswith(("G2 ", "G3 ")) else 5)
614 sent += 1
615 self.progress = sent / len(lines)
616 continue
617 size = len(line) + 1
618 while used + size > 110 and not self._stop.is_set():
619 ack = self._wait_ack(30)
620 if pending:
621 used -= pending.pop(0)
622 if ack.startswith("error"):
623 raise RuntimeError(ack)
624 if self._stop.is_set():
625 break
626 self._write_raw((line + "\n").encode())
627 pending.append(size)
628 used += size
629 sent += 1
630 self.progress = sent / len(lines)
631 while pending and not self._stop.is_set():
632 ack = self._wait_ack(60)
633 pending.pop(0)
634 if ack.startswith("error"):
635 raise RuntimeError(ack)
636 if not self._stop.is_set():
637 self.progress = 1
638 self.message = "Homen…"
639 self._prepare_home(True)
640 if not self._stop.is_set():
641 if self._arc_fallbacks:
642 self.message = "Klaar. Een boog klopte niet en is als rechte lijn getekend."
643 else:
644 self.message = "Klaar"
645 except Exception as exc:
646 self.error = str(exc)
647 self.message = str(exc)
648 self._write_raw(b"!")
649 finally:
650 self.job_active = False
651 self.job_paused = False
652 self._query()
653
654 def _job_line(self, line: str, timeout: float) -> None:
655 while self._pause.is_set() and not self._stop.is_set():
656 time.sleep(0.05)
657 if self._stop.is_set():
658 return
659 self._write_raw((line + "\n").encode())
660 ack = self._wait_ack(timeout)
661 if ack.startswith("error:33") and line.startswith(("G2 ", "G3 ")):
662 self._arc_fallbacks += 1
663 self._write_raw((chord_for_arc(line) + "\n").encode())
664 ack = self._wait_ack(timeout)
665 if not ack.startswith("error"):
666 with self._state_lock:
667 if self.error.startswith("error:33"):
668 self.error = ""
669 if ack.startswith("error"):
670 raise RuntimeError(ack)
671
672 def _wait_until_idle(self, timeout: float) -> None:
673 deadline = time.time() + timeout
674 seen_busy = False
675 idle_since = None
676 while time.time() < deadline:
677 if self._stop.is_set():
678 return
679 self._query()
680 time.sleep(0.05)
681 state = self.state.lower()
682 if state.startswith("alarm"):
683 raise RuntimeError(self.error or "Alarm")
684 if state.startswith(("run", "hold", "jog", "home", "door")):
685 seen_busy = True
686 idle_since = None
687 continue
688 if state.startswith("idle"):
689 if seen_busy or (idle_since is not None and time.time() - idle_since > 0.15):
690 return
691 if idle_since is None:
692 idle_since = time.time()
693 raise TimeoutError("De machine bleef bezig.")
694
695 def _wait_ack(self, timeout: float) -> str:
696 deadline = time.time() + timeout
697 while time.time() < deadline:
698 with self._state_lock:
699 if self._acks:
700 return self._acks.pop(0)
701 self._ack_event.wait(0.2)
702 self._ack_event.clear()
703 raise TimeoutError("De machine antwoordt niet meer.")