BlueGit

timfa timdraw

Files

timdraw / timdraw / grbl.py

grbl.py

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