sandTable/software/tests/fakegrbl.py

"""A small GRBL 1.1 simulator that behaves like a serial port, for testing without the table.

- Lines go into a 128-byte receive buffer; overflowing it raises (that would corrupt a real job).
- Lines move into a 15-block planner and are acknowledged with 'ok' as they do on GRBL.
- G1 / $J moves run in simulated time (speed-up factor), so status reports show motion.
- A home switch closes when the carriage's real position is at or below `switch_at`.
- A reset (Ctrl-X) while moving leaves it in ALARM, as on GRBL; $X clears it.
"""
import math
import re
import threading
import time

WORD = re.compile(r'([A-Z])(-?\d*\.?\d+)')


class FakeGrbl:
    RX = 128
    PLANNER = 15

    def __init__(self, speedup=50.0, carriage_mm=60.0, switch_at=2.0, jog_speedup=None):
        self.speedup = speedup
        # jogs (homing) run at most 10x real time, so the switch is found as precisely as on the
        # table (at 100x the simulated carriage overshoots it by millimetres and fakes a trip)
        self.jog_speedup = min(speedup, 10.0) if jog_speedup is None else jog_speedup
        self.out = bytearray()
        self.rx = bytearray()
        self.planner = []            # [kind, target(x,y), feed mm/min]
        self.mpos = [0.0, 0.0]
        self.wco = [0.0, 0.0]
        self.offset_true = carriage_mm   # real rho = mpos y + offset_true
        self.switch_at = switch_at
        self.state = 'Idle'
        self.hold = False
        self.alarm = False
        self.unplugged = False       # set to pretend the USB cable came out
        self.sleeping = False        # after $SLP: motors off until a reset
        self.absolute = True
        self.settings = {'$3': '0', '$100': '250.000', '$101': '250.000', '$110': '500', '$111': '500'}
        self.max_rx = 0
        self.lines = []              # every line received, in order
        self.lock = threading.Lock()
        self.alive = True
        self.t_last = time.monotonic()
        self.th = threading.Thread(target=self._run, daemon=True)
        self.th.start()
        self._emit('Grbl 1.1h [\'$\' for help]')

    # ---------------------------------------------------------- serial-port interface
    @property
    def in_waiting(self):
        if self.unplugged:
            raise OSError(5, 'Input/output error')
        with self.lock:
            return len(self.out)

    def read(self, n=1):
        if self.unplugged:
            raise OSError(5, 'Input/output error')
        t_end = time.monotonic() + 0.02
        while True:
            with self.lock:
                if self.out:
                    data = bytes(self.out[:n])
                    del self.out[:n]
                    return data
            if time.monotonic() > t_end:
                return b''
            time.sleep(0.001)

    def write(self, data):
        if self.unplugged:
            raise OSError(5, 'write failed: Input/output error')
        with self.lock:
            for b in data:
                if b == ord('?'):
                    self._status()
                elif b == ord('!'):
                    self.hold = True
                elif b == ord('~'):
                    self.hold = False
                elif b == 0x18:
                    moving = bool(self.planner) and not self.hold
                    if self.sleeping:                     # waking from sleep: GRBL comes up locked
                        self.sleeping, self.alarm = False, True
                    if moving:
                        self.alarm = True
                        self._emit('ALARM:3')            # reset while in motion: position lost
                    self.planner.clear()
                    self.rx.clear()
                    self.hold = False
                    self.wco = [0.0, 0.0]                 # GRBL clears the G92 offset on reset
                    self.state = 'Alarm' if self.alarm else 'Idle'
                    self._emit('Grbl 1.1h [\'$\' for help]')
                    if self.alarm:
                        self._emit("[MSG:'$H'|'$X' to unlock]")
                elif b == 0x85:
                    self.planner = [p for p in self.planner if p[0] != 'jog']
                else:
                    self.rx.append(b)
                    self.max_rx = max(self.max_rx, len(self.rx))
                    if len(self.rx) > self.RX:
                        raise AssertionError('GRBL receive buffer overflow')
        return len(data)

    def reset_input_buffer(self):
        with self.lock:
            self.out.clear()

    def close(self):
        self.alive = False

    # ---------------------------------------------------------- simulation
    def _emit(self, s):
        self.out += (s + '\r\n').encode()

    @property
    def pins(self):
        return 'Y' if self.mpos[1] + self.offset_true <= self.switch_at else ''

    def _status(self):
        st = self.state
        if self.hold:
            st = 'Hold:0'                # the simulator stops at once
        pn = f'|Pn:{self.pins}' if self.pins else ''
        self._emit(f'<{st}|MPos:{self.mpos[0]:.3f},{self.mpos[1]:.3f},0.000|FS:0,0|WCO:'
                   f'{self.wco[0]:.3f},{self.wco[1]:.3f},0.000{pn}>')

    def _take_line(self):
        if self.sleeping or b'\n' not in self.rx or len(self.planner) >= self.PLANNER:
            return
        raw, _, rest = bytes(self.rx).partition(b'\n')
        self.rx = bytearray(rest)
        line = raw.decode().strip().upper()
        self.lines.append(line)
        line = re.sub(r'\(.*?\)', '', line).replace(' ', '')
        if not line:
            self._emit('ok')
            return
        if line == '$$':
            for k, v in self.settings.items():
                self._emit(f'{k}={v}')
        elif line.startswith('$J='):
            body = line[3:]
            w = dict(WORD.findall(body.replace('G91', 'g').replace('G21', 'g').replace('G90', 'g')))
            rel = 'G91' in body
            tgt = list(self.mpos)
            for i, ax in enumerate('XY'):
                if ax in w:
                    tgt[i] = (self.mpos[i] + float(w[ax])) if rel else float(w[ax]) + self.wco[i]
            self.planner.append(['jog', tgt, float(w.get('F', 600))])
        elif line == '$SLP':
            self.sleeping = True
            self.planner.clear()
            self.state = 'Sleep'
        elif line.startswith('$X'):
            self.alarm = False
            self.state = 'Idle'
        elif self.alarm and not line.startswith('$'):
            self._emit('error:9')                     # G-code is locked out during an alarm
            return
        elif line.startswith('$') and '=' in line:
            k, v = line.split('=', 1)
            self.settings[k] = v
        elif line.startswith('G92'):
            w = dict(WORD.findall(line[3:]))
            for i, ax in enumerate('XY'):
                if ax in w:
                    self.wco[i] = self.mpos[i] - float(w[ax])
        elif line.startswith('G10L20P1'):
            w = dict(WORD.findall(line[8:]))
            for i, ax in enumerate('XY'):
                if ax in w:
                    self.wco[i] = self.mpos[i] - float(w[ax])
        else:
            if 'G91' in line:
                self.absolute = False
            if 'G90' in line:
                self.absolute = True
            w = dict(WORD.findall(re.sub(r'G\d+', '', line)))
            if 'X' in w or 'Y' in w:
                base = self.planner[-1][1] if self.planner else self.mpos
                tgt = list(base)
                for i, ax in enumerate('XY'):
                    if ax in w:
                        tgt[i] = (base[i] + float(w[ax])) if not self.absolute else float(w[ax]) + self.wco[i]
                self.planner.append(['move', tgt, float(w.get('F', 600))])
        self._emit('ok')

    def _run(self):
        while self.alive:
            time.sleep(0.002)
            now = time.monotonic()
            with self.lock:
                real_dt = now - self.t_last
                dt = real_dt * self.speedup
                self.t_last = now
                for _ in range(16):              # take several queued lines per tick, as fast as GRBL
                    self._take_line()
                if self.sleeping:
                    self.state = 'Sleep'
                    continue
                if self.alarm:
                    self.state = 'Alarm'
                    continue
                if self.hold or not self.planner:
                    self.state = 'Idle' if not self.planner else self.state
                    continue
                kind, tgt, feed = self.planner[0]
                self.state = 'Jog' if kind == 'jog' else 'Run'
                if kind == 'jog':
                    dt = real_dt * self.jog_speedup
                dx, dy = tgt[0] - self.mpos[0], tgt[1] - self.mpos[1]
                d = math.hypot(dx, dy)
                step = feed / 60.0 * dt
                if d <= step or d == 0:
                    self.mpos = list(tgt)
                    self.planner.pop(0)
                else:
                    self.mpos[0] += dx / d * step
                    self.mpos[1] += dy / d * step