コード例 #1
0
class MainboardControlTest(TestCase):
    callback_log = None

    def setUp(self):
        def msg_empty_callback(ctrl):
            self.callback_log.append(("empty", ctrl))

        def msg_sendable_callback(ctrl):
            self.callback_log.append(("sendable", ctrl))

        self.callback_log = []
        self.lsock, self.rsock = socket.socketpair()
        self.lsock.setblocking(False)
        self.rsock.setblocking(False)

        self.t = MainController(self.rsock.fileno(),
                                MAX_CMD_BUFSIZE,
                                msg_empty_callback=msg_empty_callback,
                                msg_sendable_callback=msg_sendable_callback)

    def assertRecv(self, msg):  # noqa
        buf = self.lsock.recv(4096)
        self.assertEqual(buf, msg)

    def assertRecvStartsWith(self, msg):  # noqa
        buf = self.lsock.recv(4096)
        self.assertEqual(buf[:len(msg)], msg)

    def send_and_process(self, buf):
        self.lsock.send(buf)
        self.t.handle_recv()

    def test_bootstrap_simple(self):
        def callback(ctrl):
            self.callback_log.append((callback, ctrl))

        self.assertFalse(self.t.ready)
        self.t.bootstrap(callback)
        self.assertRecv(b"C1O\n")
        self.assertFalse(self.t.ready)
        self.send_and_process("CTRL LINECHECK_ENABLED\n")
        self.assertTrue(self.t.ready)
        self.assertEqual(self.callback_log, [(callback, self.t)])

    def test_bootstrap_no_response(self):
        def callback(ctrl):
            self.callback_log.append((callback, ctrl))

        self.assertFalse(self.t.ready)
        self.t.bootstrap(callback)
        self.assertRecv(b"C1O\n")

        for i in range(3):
            self.t.send_timestamp = 0
            self.t.patrol()
            self.assertRecv(b"C1O\n")

        self.t.send_timestamp = 0
        self.assertRaises(SystemError, self.t.patrol)
        self.assertEqual(self.callback_log, [])

    def test_bootstrap_with_lineno_enabled(self):
        def callback(ctrl):
            self.callback_log.append((callback, ctrl))

        self.assertFalse(self.t.ready)
        self.t.bootstrap(callback)
        self.assertRecv(b"C1O\n")
        self.send_and_process(b"ER MISSING_LINENUMBER 3\n")
        self.assertRecv(b"@DISABLE_LINECHECK\n")
        self.send_and_process(b"CTRL LINECHECK_DISABLED\n")
        self.assertRecv(b"C1O\n")
        self.send_and_process("CTRL LINECHECK_ENABLED\n")
        self.assertTrue(self.t.ready)
        self.assertEqual(self.callback_log, [(callback, self.t)])

    def _bootstrap(self):
        self.t.bootstrap(None)
        self.assertRecv(b"C1O\n")
        self.send_and_process("CTRL LINECHECK_ENABLED\n")

    def test_command_full_empty_callback(self):
        self._bootstrap()

        self.t.send_cmd(b"G28")
        self.assertRecv(b"G28 N1*18\n")

        self.assertEqual(self.t.buffered_cmd_size, 1)
        self.assertEqual(self.callback_log, [])

        self.send_and_process("LN 1 1\n")
        self.assertEqual(self.t.buffered_cmd_size, 1)
        self.assertEqual(self.callback_log, [])

        self.send_and_process("LN 1 0\n")
        self.assertEqual(self.t.buffered_cmd_size, 0)
        self.assertEqual(self.callback_log, [("empty", self.t)])
        self.callback_log = []

        self.t.send_cmd(b"G28")
        self.assertRecv(b"G28 N2*17\n")
        self.send_and_process("LN 2 0\n")
        self.assertEqual(self.callback_log, [("empty", self.t)])
        self.callback_log = []

        for i in range(MAX_CMD_BUFSIZE):
            self.t.send_cmd("G1 Z%i" % i)
            self.assertRecvStartsWith("G1 Z%i N%i" % (i, i + 3))

        self.send_and_process("LN 18 16\n")
        self.assertEqual(self.t.buffered_cmd_size, 16)

        self.callback_log = []
        self.send_and_process("LN 18 15\n")
        self.assertEqual(self.t.buffered_cmd_size, 15)
        self.assertEqual(self.callback_log, [("sendable", self.t)])

        self.callback_log = []
        self.send_and_process("LN 18 5\n")
        self.assertEqual(self.t.buffered_cmd_size, 5)
        self.send_and_process("LN 18 0\n")
        self.assertEqual(self.callback_log, [("empty", self.t)])
        self.assertEqual(self.t.buffered_cmd_size, 0)

    def test_send_command_workround(self):
        self._bootstrap()
        self.t._ln = 123

        for i in range(MAX_CMD_BUFSIZE):
            self.t.send_cmd("G1 Z%i" % i)
            self.assertRecvStartsWith("G1 Z%i N%i" % (i, i + 124))

        self.assertEqual(self.t.buffered_cmd_size, 16)
        self.send_and_process("LN 128 1\n")
        self.assertEqual(self.t.buffered_cmd_size, 12)

        self.t.send_cmd("G1 Z200")
        self.assertRecvStartsWith("G1 Z200 N140")
        self.assertEqual(self.t.buffered_cmd_size, 13)
        self.send_and_process("LN 138 11\n")
        self.assertEqual(self.t.buffered_cmd_size, 13)
        self.send_and_process("LN 140 1\n")
        self.assertEqual(self.t.buffered_cmd_size, 1)

    def test_msg_full(self):
        self._bootstrap()
        self.t._ln = 256
        for i in range(MAX_CMD_BUFSIZE):
            self.t.send_cmd("G1 Z%i" % i)
            self.assertRecvStartsWith("G1 Z%i N%i" % (i, i + 257))

        self.assertRaises(RuntimeError, self.t.send_cmd, "G28")

    def test_filament_runout(self):
        self._bootstrap()
        self.assertRaises(RuntimeError, self.send_and_process,
                          "CTRL FILAMENTRUNOUT 1\n")
        self.send_and_process("CTRL FILAMENTRUNOUT 1\n")
        self.send_and_process("CTRL FILAMENTRUNOUT 1\n")

        self.t.bootstrap()
        self.assertRaises(RuntimeError, self.send_and_process,
                          "CTRL FILAMENTRUNOUT 1\n")
        self.send_and_process("CTRL FILAMENTRUNOUT 1\n")

    def test_checksum_mismatch(self):
        self._bootstrap()
        self.t._ln = 512

        self.t.send_cmd("G1 Z210")
        self.assertRecv("G1 Z210 N513*102\n")
        self.send_and_process("ER CHECKSUM_MISMATCH 513\n")
        self.assertRecv("G1 Z210 N513*102\n")

    def test_checksum_mismatch_multi(self):
        self._bootstrap()
        self.t._ln = 515

        self.t.send_cmd("G1 Z210")
        self.assertRecvStartsWith("G1 Z210 N516")
        self.t.send_cmd("G1 Z215")
        self.assertRecvStartsWith("G1 Z215 N517")
        self.t.send_cmd("G1 Z205")
        self.assertRecvStartsWith("G1 Z205 N518")

        self.send_and_process("ER CHECKSUM_MISMATCH 516\n")
        self.assertRecv(
            "G1 Z210 N516*99\nG1 Z215 N517*103\nG1 Z205 N518*105\n")
        self.send_and_process("LN 516 517\n")
        self.send_and_process("LN 516 518\n")
        self.assertRaises(socket.error, self.lsock.recv, 4096)

    def test_ln_mismatch_multi(self):
        self._bootstrap()
        self.t._ln = 601

        self.t.send_cmd("G1 X5")
        self.assertRecvStartsWith("G1 X5 N602")
        self.t.send_cmd("G1 X6")
        self.assertRecvStartsWith("G1 X6 N603")
        self.t.send_cmd("G1 X7")
        self.assertRecvStartsWith("G1 X7 N604")
        self.t.send_cmd("G1 X8")
        self.assertRecvStartsWith("G1 X8 N605")

        self.send_and_process("LN 602 0\n")
        self.send_and_process("ER LINE_MISMATCH 603 604\n")
        self.assertRecv("G1 X6 N603*99\nG1 X7 N604*101\nG1 X8 N605*107\n")
        self.send_and_process("ER LINE_MISMATCH 603 605\n")
        self.assertRaises(socket.error, self.lsock.recv, 4096)

    def test_resp_timeout(self):
        self._bootstrap()
        self.t._ln = 610
        self.t.send_cmd("G1 X8")
        self.assertRecvStartsWith("G1 X8 N611")

        for i in range(3):
            self.t.send_timestamp = 0
            self.t.patrol()
            self.assertRecvStartsWith("G1 X8 N611")

        self.t.send_timestamp = 0
        self.assertRaises(SystemError, self.t.patrol)
コード例 #2
0
class ScanTask(DeviceOperationMixIn, CommandMixIn):
    st_id = -2
    mainboard = None
    step_length = 0.45
    busying = False

    _macro = None

    def __init__(self, stack, handler, camera_id=None):
        self.camera = CameraInterface(stack)
        super(ScanTask, self).__init__(stack, handler)

        def on_mainboard_ready(ctrl):
            self.busying = False
            for cmd in ("G28", "G91", "M302", "M907 Y0.4", "T2"):
                ctrl.send_cmd(cmd)
            handler.send_text("ok")

        def on_mainboard_empty(sender):
            if self._macro:
                self._macro.on_command_empty(self)

        def on_mainboard_sendable(sender):
            if self._macro:
                self._macro.on_command_sendable(self)

        def on_mainboard_ctrl(sender, data):
            if self._macro:
                self._macro.on_ctrl_message(self, data)

        self.mainboard = MainController(
            self._sock_mb.fileno(), bufsize=14,
            empty_callback=on_mainboard_empty,
            sendable_callback=on_mainboard_sendable,
            ctrl_callback=on_mainboard_ctrl)
        self.mainboard.bootstrap(on_mainboard_ready)
        self.busying = True

    def make_gcode_cmd(self, cmd, callback=None):
        def cb():
            self._macro = None
            if callback:
                callback()
        self._macro = macro.CommandMacro(cb, (cmd, ))
        self._macro.start(self)

    def dispatch_cmd(self, handler, cmd, *args):
        if self._macro or self.busying:
            raise RuntimeError(RESOURCE_BUSY)

        elif cmd == "oneshot":
            self.oneshot(handler)

        elif cmd == "scanimages":
            self.take_images(handler)

        elif cmd == "scan_check":
            self.scan_check(handler)

        elif cmd == "get_cab":
            self.get_cab(handler)

        elif cmd == "calibrate":
            self.async_calibrate(handler)

        elif cmd == "scanlaser":
            param = args[0] if args else ""
            l_on = "l" in param
            r_on = "r" in param

            def cb():
                handler.send_text("ok")
            self.change_laser(left=l_on, right=r_on, callback=cb)

        elif cmd == "set":
            if args[0] == "steplen":
                self.step_length = float(args[1])
                handler.send_text("ok")
            else:
                raise RuntimeError(UNKNOWN_COMMAND, args[1])

        elif cmd == "scan_backward":
            def cb():
                self._macro = None
                handler.send_text("ok")
            cmd = "G1 F500 E-%.5f" % self.step_length
            self._macro = macro.CommandMacro(cb, (cmd, ))
            self._macro.start(self)

        elif cmd == "scan_next":
            def cb():
                self._macro = None
                handler.send_text("ok")
            cmd = "G1 F500 E%.5f" % self.step_length
            self._macro = macro.CommandMacro(cb, (cmd, ))
            self._macro.start(self)

        elif cmd == "quit":
            self.stack.exit_task(self)
            handler.send_text("ok")

        else:
            logger.debug("Can not handle: '%s'" % cmd)
            raise RuntimeError(UNKNOWN_COMMAND)

    def change_laser(self, left, right, callback=None):
        def cb():
            self._macro = None
            if callback:
                callback()

        flag = (1 if left else 0) + (2 if right else 0)
        self._macro = macro.CommandMacro(cb, ("X1E%i" % flag, ))
        self._macro.start(self)

        if not callback:
            while self._macro:
                rl = select((self._sock_mb, ), (), (), 1.0)[0]
                if rl:
                    self.on_mainboard_message(self._watcher_mb, 0)

    def scan_check(self, handler):
        def callback(m):
            self.busying = False
            handler.send_text(m)
        self.camera.async_check_camera_position(callback)
        self.busying = True

    def async_calibrate(self, handler):
        # this is measure by data set
        table = {8: 60, 7: 51, 6: 40, 5: 32, 4: 26, 3: 19, 2: 11, 1: 6, 0: 1}
        compute_cab_ref = (("O", False, False),
                           ("L", True, False),
                           ("R", False, True))

        data = {"flag": 0, "thres": 0.2, "calibrate_param": []}

        def on_loop(output=None):
            if output:
                self.change_laser(left=False, right=False)
                self.busying = False
                handler.send_text('ok ' + output)

            elif data["flag"] < 10:
                data["flag"] += 1
                self.camera.async_get_bias(on_get_bias)
            else:
                self.change_laser(left=False, right=False)
                self.busying = False
                handler.send_text('ok fail chess')

        def on_compute_cab(step, m):
            m = m.split()[1]
            data["calibrate_param"].append(m)
            if len(data["calibrate_param"]) < 3:
                begin_compute_cab()
            else:
                if 'fail' in data["calibrate_param"]:
                    output = ' '.join(data["calibrate_param"])
                    on_loop('fail laser ' + output)
                elif all(abs(float(r) - float(data["calibrate_param"][0])) < 72
                         for r in data["calibrate_param"][1:]):
                    # so naive check
                    s = Storage('camera')
                    s['calibration'] = ' '.join(
                        map(lambda x: str(round(float(x))),
                            data["calibrate_param"]))
                    output = ' '.join(data["calibrate_param"])
                    on_loop(output)
                else:
                    output = ' '.join(data["calibrate_param"])
                    on_loop('fail laser ' + output)

        def begin_compute_cab():
            step, l, r = compute_cab_ref[len(data["calibrate_param"])]
            logger.debug("calibrate laser step %s", step)
            self.change_laser(left=l, right=r)
            self.camera.async_compute_cab(step, on_compute_cab)

        def on_get_bias(m):
            data["flag"] += 1
            w = float(m.split()[1])
            logger.debug("Camera calibrate w = %s", w)
            if isnan(w):
                on_loop()
            else:
                if abs(w) < data["thres"]:  # good enough to calibrate
                    begin_compute_cab()
                elif w < 0:
                    self.make_gcode_cmd(
                        "G1 F500 E{}".format(table.get(round(abs(w)), 60)),
                        on_loop)
                elif w > 0:
                    self.make_gcode_cmd(
                        "G1 F500 E-{}".format(table.get(round(abs(w)), 60)),
                        on_loop)
                data["thres"] += 0.05

        on_loop()
        self.busying = True

    def get_cab(self, handler):
        s = Storage('camera')
        a = s.readall('calibration')
        if a is None:
            a = '320 320 320'
        handler.send_text("ok " + a)

    def oneshot(self, handler):
        def sent_callback(h):
            self.busying = False
            handler.send_text("ok")

        def recv_callback(result):
            mimetype, length, stream = result
            handler.async_send_binary(mimetype, length, stream, sent_callback)

        self.camera.async_oneshot(recv_callback)
        self.busying = True

    def take_images(self, handler):
        def cb_complete(h):
            self.busying = False
            handler.send_text("ok")

        def cb_shot3_ready(result):
            mimetype, length, stream = result
            handler.async_send_binary(mimetype, length, stream, cb_complete)

        def cb_shot3(h):
            self.camera.async_oneshot(cb_shot3_ready)

        def cb_shot2_ready(result):
            mimetype, length, stream = result
            self.change_laser(left=False, right=False,
                              callback=lambda: sleep(0.04))
            handler.async_send_binary(mimetype, length, stream, cb_shot3)

        def cb_shot2(h):
            self.camera.async_oneshot(cb_shot2_ready)

        def cb_shot1_ready(result):
            mimetype, length, stream = result
            self.change_laser(left=False, right=True,
                              callback=lambda: sleep(0.04))
            handler.async_send_binary(mimetype, length, stream, cb_shot2)

        def cb_shot1():
            self.camera.async_oneshot(cb_shot1_ready)

        self.change_laser(left=True, right=False, callback=cb_shot1)
        self.busying = True

    def on_mainboard_message(self, watcher, revent):
        try:
            self.mainboard.handle_recv()
        except IOError as e:
            if e.errno == EAGAIN:
                return
            logger.exception("Mainboard connection broken")
            self.handler.send_text("error SUBSYSTEM_ERROR")
            self.stack.exit_task(self)
        except RuntimeError:
            pass
        except Exception:
            logger.exception("Unhandle Error")

    def on_timer(self, watcher, revent):
        metadata.update_device_status(self.st_id, 0, "N/A",
                                      self.handler.address)

    def clean(self):
        try:
            if self.mainboard:
                if self.mainboard.ready:
                    self.mainboard.send_cmd("X1E0")
                self.mainboard.close()
                self.mainboard = None
        except Exception:
            logger.exception("Mainboard error while quit")

        if self.camera:
            self.camera.close()
            self.camera = None

        metadata.update_device_status(0, 0, "N/A", "")
コード例 #3
0
class MaintainTask(DeviceOperationMixIn, CommandMixIn):
    st_id = -1
    mainboard = None
    toolhead = None
    busying = False
    toolhead_updating = False
    _has_zprobe = False

    def __init__(self, stack, handler):
        super(MaintainTask, self).__init__(stack, handler)

        self.busying = False
        self._has_zprobe = False

        self._macro = None
        self._on_macro_error = None
        self._on_macro_running = None

        def on_mainboard_ready(_):
            self.busying = False
            handler.send_text("ok")

        def on_mainboard_empty(sender):
            if self._macro:
                self._macro.on_command_empty(self)

        def on_mainboard_sendable(sender):
            if self._macro:
                self._macro.on_command_sendable(self)

        def on_mainboard_ctrl(sender, data):
            if self._macro:
                self._macro.on_ctrl_message(self, data)

        self.mainboard = MainController(
            self._sock_mb.fileno(),
            bufsize=14,
            empty_callback=on_mainboard_empty,
            sendable_callback=on_mainboard_sendable,
            ctrl_callback=on_mainboard_ctrl)
        self.toolhead = HeadController(self._sock_th.fileno())

        self.mainboard.bootstrap(on_mainboard_ready)
        self.toolhead.bootstrap(self.on_toolhead_ready)

    def on_toolhead_ready(self, sender):
        if sender.module_name and sender.module_name != "N/A":
            tools.toolhead_on()

    def on_mainboard_message(self, watcher, revent):
        try:
            self.mainboard.handle_recv()

        except IOError as e:
            from errno import EAGAIN
            if e.errno == EAGAIN:
                # TODO: There is a recv bug in C code, this is a quit fix to
                # passthrough it.
                return
            logger.exception("Mainboard connection broken")
            if self.mainboard.ready:
                self.handler.send_text("error SUBSYSTEM_ERROR")
                self.handler.close()
            else:
                self.handler.send_text("error SUBSYSTEM_ERROR")
                self.stack.exit_task(self)
        except (RuntimeError, SystemError) as e:
            if self._macro:
                self._on_macro_error(e)
        except Exception:
            logger.exception("Unhandle Error")

    def on_headboard_message(self, watcher, revent):
        try:
            self.toolhead.handle_recv()
        except IOError:
            logger.exception("Toolhead connection broken")
            self.stack.exit_task(self)

        except (HeadOfflineError, HeadResetError) as e:
            logger.debug("Head reset")
            tools.toolhead_standby()
            self.toolhead.bootstrap(self.on_toolhead_ready)
            if self._macro:
                self._on_macro_error(e)

        except HeadError as e:
            logger.info("Head Error: %s", e)

        except SystemError as e:
            logger.exception("Unhandle Error")
            if self._macro:
                self._on_macro_error(e)

        except Exception:
            logger.exception("Unhandle Error")

    def dispatch_cmd(self, handler, cmd, *args):
        if cmd == "stop_load_filament":
            self.mainboard.send_cmd("@HOME_BUTTON_TRIGGER\n", raw=1)
            return
        elif self.busying:
            raise RuntimeError(RESOURCE_BUSY)

        if cmd == "home":
            self.do_home(handler)

        elif cmd == "move":
            try:
                opt = {
                    k: float(v)
                    for k, v in (arg.split(':', 1) for arg in args)
                }
                self.do_move(handler, **opt)
            except (ValueError, IndexError):
                raise RuntimeError(UNKNOWN_COMMAND)

        elif cmd == "calibration" or cmd == "calibrate":
            try:
                threshold = float(args[0])
                if threshold < 0.01:
                    threshold = 0.01
            except (ValueError, IndexError):
                threshold = float("inf")

            clean = "clean" in args
            self.do_calibrate(handler, threshold, clean=clean)

        elif cmd == "zprobe":
            if len(args) > 0:
                h = float(args[0])
                self.do_zprobe(handler, h=h)
            else:
                self.do_zprobe(handler)

        elif cmd == "load_filament":
            self.do_load_filament(handler, int(args[0]), float(args[1]))

        elif cmd == "load_flexible_filament":
            self.do_load_filament(handler, int(args[0]), float(args[1]), True)

        elif cmd == "unload_filament":
            self.do_unload_filament(handler, int(args[0]), float(args[1]))

        elif cmd == "headinfo":
            self.head_info(handler)

        elif cmd == "headstatus":
            self.head_status(handler)

        elif cmd == "reset_mb":
            reset_mb()
            self.stack.exit_task(self)
            handler.send_text("ok")

        elif cmd == "extruder_temp":
            self.do_change_extruder_temperature(handler, *args)

        elif cmd == "diagnosis_sensor":
            self.diagnosis_sensor(handler)

        elif cmd == "update_head":
            self.update_toolhead_fw(handler, *args)

        elif cmd == "hal_diagnosis":
            self.do_hal_diagnosis(handler)

        elif cmd == "quit":
            self.stack.exit_task(self)
            handler.send_text("ok")
        else:
            logger.debug("Can not handle: '%s'" % cmd)
            raise RuntimeError(UNKNOWN_COMMAND)

    def diagnosis_sensor(self, handler):
        dataset = []

        def on_message_cb(msg):
            if msg.startswith("DATA "):
                kv = msg[5:].split(" ", 1)
                if len(kv) == 2:
                    dataset.append("%s=%s" % (kv[0], kv[1]))
                else:
                    dataset.append(kv)

        def on_success_cb():
            handler.send_text("ok " + "\x00".join(dataset))
            self._macro = self._on_macro_error = self._on_macro_running = None
            self.busying = False

        def on_macro_error(error):
            self._macro.giveup(self)
            self._macro = self._on_macro_error = self._on_macro_running = None
            handler.send_text("error %s" % " ".join(error.args))
            self.busying = False

        self._macro = macro.CommandMacro(on_success_cb, ["M666L1"],
                                         on_message_cb)
        self._on_macro_error = on_macro_error
        self.busying = True
        self._macro.start(self)

    def do_change_extruder_temperature(self, handler, sindex, stemp):
        if not self.toolhead.ready or not self.toolhead.sendable():
            raise HeadError(EXEC_HEAD_ERROR, RESOURCE_BUSY)
        module = self.toolhead.status["module"]
        if module != "EXTRUDER":
            raise HeadTypeError("EXTRUDER", module)

        self.toolhead.ext.set_heater(int(sindex), float(stemp))
        handler.send_text("ok")

    def do_move(self, handler, **kw):
        def on_success_cb():
            handler.send_text("ok")
            self._macro = self._on_macro_error = self._on_macro_running = None
            self.busying = False

        def on_macro_error(error):
            self._macro.giveup(self)
            self._macro = self._on_macro_error = self._on_macro_running = None
            handler.send_text("error %s" % " ".join(error.args))
            self.busying = False

        subcmd = ''.join(MOVE_COMMAND[k] % v for k, v in kw.items()
                         if k in MOVE_COMMAND)
        if 'E' in subcmd:
            cmds = ['T2', 'G92E0', 'G1' + subcmd, 'T0']
        else:
            cmds = ['G1' + subcmd]

        self._macro = macro.CommandMacro(on_success_cb, cmds)
        self._on_macro_error = on_macro_error
        self._macro.start(self)
        self.busying = True

    def do_load_filament(self, handler, index, temp, disable_accelerate=False):
        if not self.toolhead.ready or not self.toolhead.sendable():
            raise HeadError(EXEC_HEAD_ERROR, RESOURCE_BUSY)
        module = self.toolhead.status["module"]
        if module != "EXTRUDER":
            raise HeadTypeError("EXTRUDER", module)

        def on_load_done():
            handler.send_text("ok")
            self._macro = self._on_macro_error = self._on_macro_running = None
            self.busying = False

        def on_macro_error(error):
            self._macro.giveup(self)
            self._macro = self._on_macro_error = self._on_macro_running = None
            self.busying = False
            handler.send_text("error %s" % " ".join(error.args))

        def on_heating_done():
            def on_message(msg):
                try:
                    if msg == "FILAMENT+":
                        handler.send_text("CTRL LOADING")
                    elif msg == "FILAMENT-":
                        handler.send_text("CTRL WAITING")
                except Exception:
                    self.mainboard.send_cmd("@HOME_BUTTON_TRIGGER\n")
                    raise

            opt = Options(head="EXTRUDER")
            if opt.plus_extrusion:
                self.mainboard.send_cmd("M92E145")
            self._macro = macro.LoadFilamentMacro(on_load_done, index,
                                                  opt.filament_detect,
                                                  disable_accelerate,
                                                  on_message)
            self._macro.start(self)

        def on_macro_running():
            if isinstance(self._macro, macro.ControlHeaterMacro):
                rt = self.toolhead.status.get("rt")
                if rt:
                    try:
                        handler.send_text("CTRL HEATING %.1f" % rt[index])
                    except IndexError:
                        pass

        self._macro = macro.ControlHeaterMacro(on_heating_done, index, temp)
        self._on_macro_error = on_macro_error
        self._on_macro_running = on_macro_running
        self.busying = True
        self._macro.start(self)
        handler.send_text("continue")

    def do_unload_filament(self, handler, index, temp):
        if not self.toolhead.ready:
            raise HeadError(EXEC_HEAD_ERROR, RESOURCE_BUSY)
        module = self.toolhead.status["module"]
        if module != "EXTRUDER":
            raise HeadTypeError("EXTRUDER", module)

        def on_load_done():
            handler.send_text("ok")
            self._macro = self._on_macro_error = self._on_macro_running = None
            self.busying = False

        def on_heating_done():
            self._macro = macro.UnloadFilamentMacro(on_load_done, index)
            self._macro.start(self)

        def on_macro_error(error):
            self._macro.giveup(self)
            self._macro = self._on_macro_error = self._on_macro_running = None
            self.busying = False
            handler.send_text("error %s" % " ".join(error.args))

        def on_macro_running():
            if isinstance(self._macro, macro.WaitHeadMacro):
                st = self.toolhead.status.copy()
                handler.send_text("CTRL HEATING %.1f" % st.get("rt")[index])
            else:
                handler.send_text("CTRL UNLOADING")

        self._macro = macro.ControlHeaterMacro(on_heating_done, index, temp)
        self._on_macro_error = on_macro_error
        self._on_macro_running = on_macro_running
        self.busying = True
        self._macro.start(self)
        handler.send_text("continue")

    def do_home(self, handler):
        def on_success_cb():
            handler.send_text("ok")
            self._macro = self._on_macro_error = self._on_macro_running = None
            self.busying = False

        def on_macro_error(error):
            self._macro.giveup(self)
            self._macro = self._on_macro_error = self._on_macro_running = None
            handler.send_text("error %s" % " ".join(error.args))
            self.busying = False

        cor = Preference.instance().plate_correction
        self._macro = macro.CommandMacro(on_success_cb, [
            "M666X%(X).4fY%(Y).4fZ%(Z).4fR%(R).4fD%(D).5fH%(H).4f" % cor,
            "G28+"
        ])
        self._on_macro_error = on_macro_error
        self.busying = True
        self._macro.start(self)

    def do_calibrate(self, handler, threshold, clean=False):
        def on_success_cb():
            while self._macro.debug_logs:
                handler.send_text("DEBUG " + self._macro.debug_logs.popleft())

            p1, p2, p3 = self._macro.history[-1]
            handler.send_text("ok %.4f %.4f %.4f" % (p1, p2, p3))
            self._macro = self._on_macro_error = self._on_macro_running = None
            self.busying = False

        def on_macro_error(error):
            self._macro.giveup(self)
            self._macro = self._on_macro_error = self._on_macro_running = None
            self.busying = False
            handler.send_text("error %s" % " ".join(error.args))

        def on_macro_running():
            while self._macro.debug_logs:
                handler.send_text("DEBUG " + self._macro.debug_logs.popleft())
            handler.send_text("CTRL POINT %i" % len(self._macro.data))

        opt = Options(head="EXTRUDER")

        correct_at_final = True if threshold == float("inf") else False

        if self._has_zprobe is False:
            self._has_zprobe = True
            opt = Options(head="EXTRUDER")
            self._macro = macro.CorrectionMacro(
                on_success_cb,
                threshold=threshold,
                clean=clean,
                dist=opt.zprobe_dist,
                correct_at_final=correct_at_final)
        else:
            self._macro = macro.CorrectionMacro(
                on_success_cb,
                threshold=threshold,
                clean=clean,
                correct_at_final=correct_at_final)

        self._on_macro_error = on_macro_error
        self._on_macro_running = on_macro_running
        self._on_macro_error = on_macro_error
        self.busying = True
        self._macro.start(self)
        handler.send_text("continue")

    def do_zprobe(self, handler, h=None):
        def on_success_cb():
            while self._macro.debug_logs:
                handler.send_text("DEBUG " + self._macro.debug_logs.popleft())
            handler.send_text("ok %.4f" % self._macro.history[0])
            self._macro = self._on_macro_error = self._on_macro_running = None
            self.busying = False

        def on_macro_error(error):
            self._macro.giveup(self)
            self._macro = self._on_macro_error = self._on_macro_running = None
            self.busying = False
            handler.send_text("error %s" % " ".join(error.args))

        def on_macro_running():
            handler.send_text("CTRL ZPROBE")

        if self._has_zprobe is False:
            self._has_zprobe = True
            opt = Options(head="EXTRUDER")
            self._macro = macro.ZprobeMacro(on_success_cb,
                                            threshold=float("inf"),
                                            dist=opt.zprobe_dist)
        else:
            self._macro = macro.ZprobeMacro(on_success_cb,
                                            threshold=float("inf"))
        self._on_macro_running = on_macro_running
        self._on_macro_error = on_macro_error
        self.busying = True
        self._macro.start(self)
        handler.send_text("continue")

    def do_hal_diagnosis(self, handler):
        memory_stack = []

        def on_resp(ret, data):
            try:
                io_w, timer_w, sock = data
                io_w.stop()
                timer_w.stop()
                sock.close()
            finally:
                handler.send_text("ok %s" % ret)
                self.busying = False
                while memory_stack:
                    memory_stack.pop()

        def on_io(watcher, revent):
            ret = tools.hal_diagnosis_result(watcher.data[-1])
            on_resp(ret, watcher.data)

        def on_timeout(watcher, revent):
            on_resp("HAL_TIMEOUT", watcher.data)

        try:
            sock = tools.begin_hal_diagnosis()
            self.busying = True

            io_watcher = self.stack.loop.io(sock, EV_READ, on_io)
            t_watcher = self.stack.loop.timer(90, 0, on_timeout)
            memory_stack.append(io_watcher)
            memory_stack.append(t_watcher)
            memory_stack.append(sock)

            io_watcher.data = t_watcher.data = (io_watcher, t_watcher, sock)
            io_watcher.start()
            t_watcher.start()
        except Exception:
            logger.exception("HAL diagnosis error")
            handler.send_text("error SUBSYSTEM_ERROR")

    def head_info(self, handler):
        dataset = self.toolhead.profile.copy()
        dataset["version"] = dataset.get("VERSION")
        dataset["module"] = dataset.get("TYPE")
        handler.send_text("ok " + dumps(dataset))

    def head_status(self, handler):
        handler.send_text("ok " + dumps(self.toolhead.status))

    def update_toolhead_fw(self, handler, mimetype, sfilesize):
        def ret_callback(success):
            logger.debug("Toolhead update retuen: %s", success)
            self.toolhead_updating = False

        filesize = int(sfilesize)
        if filesize > (1024 * 256):
            raise RuntimeError(TOO_LARGE)

        t = UpdateHbFwTask(self.stack, handler, filesize)
        self.stack.enter_task(t, ret_callback)
        handler.send_text("continue")
        self.toolhead_updating = True

    def on_timer(self, watcher, revent):
        metadata.update_device_status(self.st_id, 0, "N/A",
                                      self.handler.address)

        if self.toolhead_updating:
            return

        try:
            self.mainboard.patrol()
            self.toolhead.patrol()

            if self._on_macro_running:
                self._on_macro_running()

        except (HeadOfflineError, HeadResetError) as e:
            logger.info("%s", e)
            tools.toolhead_standby()
            if self._macro:
                self._on_macro_error(e)
            self.toolhead.bootstrap(self.on_toolhead_ready)

        except RuntimeError as e:
            logger.info("%s", e)
            if self._macro:
                self._on_macro_error(e)

        except SystemError:
            if self.busying:
                self.handler.send_text("error %s" % SUBSYSTEM_ERROR)
                self.stack.exit_task(self)
            else:
                logger.exception("Mainboard dead during maintain")
                self.handler.send_text("error %s" % SUBSYSTEM_ERROR)
                self.handler.close()

        except IOError:
            logger.warn("Socket IO Error")
            self.handler.close()

        except Exception:
            logger.exception("Unhandle error")
            self.handler.close()

    def clean(self):
        try:
            if self.mainboard:
                self.mainboard.send_cmd("@HOME_BUTTON_TRIGGER\n", raw=1)
                self.mainboard.close()
                self.mainboard = None
        except Exception:
            logger.exception("Mainboard error while quit")

        try:
            if self.toolhead:
                if self.toolhead.ready:
                    # > Check should toolhead power deplayoff
                    if self.toolhead.module_name == "EXTRUDER":
                        for t in self.toolhead.status.get("rt", ()):
                            if t > 60:
                                logger.debug("Set toolhead delay off")
                                metadata.delay_toolhead_poweroff = b"\x01"
                                break
                    # ^
                    self.toolhead.shutdown()
                self.toolhead = None
        except Exception:
            logger.exception("Toolhead error while quit")

        try:
            tools.toolhead_standby()
        except Exception:
            logger.exception("HAL control error while quit")

        metadata.update_device_status(0, 0, "N/A", "")
コード例 #4
0
class IControlTask(DeviceOperationMixIn):
    st_id = -3  # Device status ID
    main_e_axis = 0  # E axis control
    cmd_index = 0  # Command counter
    cmd_queue = None  # Command store queue
    udp_sock = None  # UDP socket to send status
    handler = None  # Client TCP connection object
    known_position = None  # Is toolhead position is known or not
    mainboard = None  # Mainborad Controller
    toolhead = None  # Headboard Controller
    head_resp_stack = None  # Toolhead raw rasponse stack

    def __init__(self, stack, handler):
        super(IControlTask, self).__init__(stack, handler)
        self.handler = proxy(handler)
        self.handler.binary_mode = True
        self.cmd_queue = deque()
        self.meta = Metadata.instance()

        self._ready = 0

        def on_mainboard_ready(ctrl):
            self._ready |= 1
            self.mainboard.send_cmd("X8F")
            self.mainboard.send_cmd("T0")
            self.mainboard.send_cmd("G90")
            self.mainboard.send_cmd("G92E0")
            handler.send_text("ok")

        self.mainboard = MainController(
            self._sock_mb.fileno(),
            bufsize=14,
            empty_callback=self.on_mainboard_empty,
            sendable_callback=self.on_mainboard_sendable,
            ctrl_callback=self.on_mainboard_result)
        self.toolhead = HeadController(
            self._sock_th.fileno(),
            msg_callback=self.toolhead_message_callback)

        self.mainboard.bootstrap(on_mainboard_ready)
        self.unpacker = Unpacker()

    def on_toolhead_ready(self, ctrl):
        self._ready |= 2

    @property
    def buflen(self):
        return len(self.cmd_queue) + self.mainboard.buffered_cmd_size

    def on_mainboard_empty(self, caller):
        self.fire()

    def on_mainboard_sendable(self, caller):
        self.fire()

    def toolhead_message_callback(self, sender, data):
        if data and self.head_resp_stack is not None and \
                len(self.head_resp_stack) <= 32:
            self.head_resp_stack.append(data)
            self.send_udp1(sender)

    def on_binary(self, buf, handler):
        self.unpacker.feed(buf)
        for payload in self.unpacker:
            self.process_cmd(handler, *payload)

    def process_cmd(self, handler, index, cmd, *params):
        if index != self.cmd_index:
            logger.debug("Ignore %s 0x%02x %s", index, cmd, params)
            return

        fn = CMD_MATRIX.get(cmd)
        try:
            if cmd < 0xf0:
                fn(self, handler, *params)
            else:
                fn(self, handler, *params)

            self.cmd_index += 1

        except InternalError as e:
            self.handler.send(packb((0xff, self.cmd_index, e[1])))

        except Exception:
            logger.exception("Unknown error during processing command")
            self.handler.send(packb((0xff, self.cmd_index, MSG_UNKNOWN_ERROR)))
            self.on_require_kill(handler)

    def fire(self):
        if self.cmd_queue:
            target, cmd = self.cmd_queue[0]
            if target == TARGET_MAINBOARD:
                if self.mainboard.queue_full:
                    return
                else:
                    self.cmd_queue.popleft()
                    self.mainboard.send_cmd(cmd)
            elif target == TARGET_TOOLHEAD:
                if self.mainboard.buffered_cmd_size == 0:
                    if self.toolhead.sendable():
                        self.cmd_queue.popleft()
                        # TODO
                        self.toolhead.send_cmd(cmd, self)
                else:
                    return

    def on_mainboard_message(self, watcher, revent):
        try:
            self.mainboard.handle_recv()
        except IOError:
            logger.error("Mainboard connection broken")
            self.stack.exit_task(self)
            self.send_udp0()
        except Exception:
            logger.exception("Unhandle Error")

    def on_headboard_message(self, watcher, revent):
        try:
            self.toolhead.handle_recv()
            check_toolhead_errno(self.toolhead, self.th_error_flag)
            self.fire()

        except IOError:
            logger.error("Headboard connection broken")
            self.stack.exit_task(self)

        except (HeadResetError, HeadOfflineError, HeadTypeError):
            self._ready &= ~2

        except HeadError as e:
            logger.info("Head Error: %s", e)

        except Exception:
            logger.exception("Unhandle Error")

    def on_mainboard_result(self, controller, message):
        # Note: message will be...
        #   "DATA HOME 12.3 -23.2 122.3"
        if message.startswith("DATA HOME"):
            position = [float(val) for val in message[10:].split(" ")]
            if float("nan") in position:
                self.handler.send(packb((CMD_G028, 1, None)))
            else:
                self.handler.send(packb((CMD_G028, 0, position)))
                self.known_position = [0, 0, 240]

        #   "DATA READ X:0.124 Y:0.234 Z:0.534 F0:1 F1:0 MB:0"
        if message.startswith("DATA READ "):
            output = {}
            for key, val in ((p.split(":") for p in message[10:].split(" "))):
                if key in ("X", "Y", "Z"):
                    output[key] = float(val)
                elif key in ("F0", "F1"):
                    output[key] = (val == "1")
                elif key == "MB":
                    output[key] = (val == "1")
            self.handler.send(packb((CMD_VALU, output)))
        #   "DATA ZPROBE -0.5"
        if message.startswith("DATA ZPROBE "):
            self.handler.send(packb((CMD_G030, float(message[12:]))))

    def send_udp0(self):
        if self.udp_sock:
            try:
                buf = packb((0, "", self.cmd_index, self.buflen))
                self.udp_sock.send(buf)
            except socket.error:
                pass

    def send_udp1(self, toolhead):
        if self.udp_sock:
            try:
                if self.head_resp_stack is not None:
                    buf = packb((2, "", 0, len(self.head_resp_stack)))
                    self.udp_sock.send(buf)

                if toolhead.ready:
                    buf = packb(
                        (1, "", 0, toolhead.error_code, toolhead.status))
                    self.udp_sock.send(buf)

                # elif toolhead.ready_flag > 0:
                #     buf = packb((1, "", 0, -1, {}))
                #     self.udp_sock.send(buf)

                else:
                    buf = packb((1, "", 0, -2, {}))
                    self.udp_sock.send(buf)

            except socket.error:
                pass

    def send_udps(self, signal):
        if self.udp_sock:
            try:
                self.udp_sock.send(packb((signal, )))
            except socket.error:
                pass

    def on_timer(self, watcher, revent):
        self.meta.update_device_status(self.st_id, 0, "N/A",
                                       self.handler.address)

        self.send_udp0()
        if not self._ready & 2:
            self.send_udp1(self.toolhead)

        try:
            self.mainboard.patrol()
        except RuntimeError as e:
            logger.info("%s", e)

        except Exception:
            logger.exception("Mainboard dead")
            self.handler.send_text(packb((0xff, -1, 0xff, SUBSYSTEM_ERROR)))
            self.on_require_kill(self.handler)
            return

        try:
            self.toolhead.patrol()
        except (HeadOfflineError, HeadResetError) as e:
            logger.debug("Head Offline/Reset: %s", e)

        except RuntimeError as e:
            logger.info("%s", e)

        except socket.error:
            logger.warn("Socket IO Error")
            self.handler.close()

        except Exception:
            logger.exception("Toolhead dead")
            self.handler.send_text(packb((0xff, -1, 0xff, SUBSYSTEM_ERROR)))
            self.on_require_kill(self.handler)
            return

    def clean(self):
        self.mainboard.send_cmd("@HOME_BUTTON_TRIGGER\n")

        if self.toolhead:
            if self.toolhead.ready:
                self.toolhead.shutdown()
            self.toolhead = None

        if self.mainboard:
            self.mainboard.close()
            self.mainboard = None

        self.handler.binary_mode = False

    def append_cmd(self, target, cmd):
        self.cmd_queue.append((target, cmd))
        self.fire()

    def create_movement_command(self,
                                F=None,
                                X=None,
                                Y=None,
                                Z=None,
                                E0=None,
                                E1=None,
                                E2=None):  # noqa
        target = self.known_position
        yield "G1"

        if F:
            yield "F%i" % F

        if X is not None or Y is not None or Z is not None:
            if self.known_position:
                if X is not None:
                    target[0] = X
                    yield "X%.5f" % X
                if Y is not None:
                    target[1] = Y
                    yield "Y%.5f" % Y
                if Z is not None:
                    target[2] = Z
                    yield "Z%.5f" % Z

                if (target[0]**2 + target[1]**2) > 28900:
                    raise InternalError(CMD_G001, MSG_OPERATION_ERROR)
                elif target[2] > 240 or target[2] < 0:
                    raise InternalError(CMD_G001, MSG_OPERATION_ERROR)

            else:
                raise InternalError(CMD_G001, MSG_OPERATION_ERROR)

        eflag = False
        for i, e in ((0, E0), (1, E1), (2, E2)):
            if e is not None:
                if eflag:
                    raise InternalError(CMD_G001, MSG_OPERATION_ERROR)
                else:
                    eflag = True
                    if self.main_e_axis != i:
                        yield "T%i" % i
                        self.main_e_axis = i
                    yield "E%.5f" % e

        self.known_position = target

    def on_move(self, handler, kw):
        try:
            cmd = "".join(self.create_movement_command(**kw))
            self.append_cmd(TARGET_MAINBOARD, cmd)
        except TypeError:
            raise InternalError(CMD_G001, MSG_BAD_PARAMS)

    def on_sleep(self, handler, secondes):
        try:
            cmd = "G4S%.4f" % secondes
            self.append_cmd(TARGET_MAINBOARD, cmd)
        except TypeError:
            raise InternalError(CMD_G004, MSG_BAD_PARAMS)

    def on_scan_lasr(self, handler, flags):
        try:
            cmd = "X1E%i" % flags
            self.append_cmd(TARGET_MAINBOARD, cmd)
        except TypeError:
            raise InternalError(CMD_SLSR, MSG_BAD_PARAMS)

    def on_home(self, handler):
        self.append_cmd(TARGET_MAINBOARD, "X6")
        self.known_position = None

    def on_lock_motors(self, handler):
        self.append_cmd(TARGET_MAINBOARD, "M17")

    def on_release_motors(self, handler):
        self.append_cmd(TARGET_MAINBOARD, "M84")
        self.known_position = None

    def on_z_probe(self, handler, x, y):
        try:
            if self.known_position and x**2 + y**2 <= 7225:
                cmd = "G30X%.5fY%.5f" % (x, y)
                self.append_cmd(TARGET_MAINBOARD, cmd)

            else:
                raise InternalError(CMD_G030, MSG_OPERATION_ERROR)
        except TypeError:
            raise InternalError(CMD_G030, MSG_BAD_PARAMS)

    # def on_adjust(self, handler, kw):
    #     pass

    def on_set_toolhead_temperature(self, handler, index, temperature):
        if index == 0 and temperature >= 0 and temperature <= 220:
            cmd = "H%i%.1f" % (index, temperature)
            self.append_cmd(TARGET_TOOLHEAD, cmd)
        else:
            raise InternalError(CMD_M104, MSG_OPERATION_ERROR)

    def on_set_toolhead_fan_speed(self, handler, index, speed):
        if index == 0 and speed >= 0 and speed <= 1:
            cmd = "F%i%.3f" % (index, speed)
            self.append_cmd(TARGET_TOOLHEAD, cmd)
        else:
            raise InternalError(CMD_M106, MSG_OPERATION_ERROR)

    def on_set_toolhead_pwm(self, handler, pwm):
        if pwm >= 0 and pwm <= 1:
            cmd = "X2O" % (pwm * 255)
            self.append_cmd(TARGET_MAINBOARD, cmd)
        else:
            raise InternalError(CMD_HLSR, MSG_OPERATION_ERROR)

    def on_query_value(self, handler, flags):
        self.append_cmd(TARGET_MAINBOARD, "X87F%i" % flags)

    def on_toolhead_profile(self, handler):
        buf = packb((CMD_THPF, self.toolhead.info()))
        self.handler.send(buf)

    def on_toolhead_raw_command(self, handler, cmd):
        self.append_cmd(TARGET_TOOLHEAD, cmd)

    def on_toolhead_raw_response(self, handler):
        buf = packb((CMD_THRR, self.head_resp_stack))
        self.head_resp_stack = []
        self.handler.send(buf)

    def on_require_sync(self, handler, ipaddr, port, salt):
        endpoint = (ipaddr, port)
        logger.debug("Create sync udp endpoint at %s", repr(endpoint))
        try:
            s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
            s.connect(endpoint)
            s.send(packb((0xff, )))
        except (TypeError, OSError):
            raise InternalError(CMD_SYNC, MSG_OPERATION_ERROR)

        try:
            if self.udp_sock:
                self.udp_sock.close()
        finally:
            self.udp_sock = s

    def on_require_head(self, handler, head_type):
        self.toolhead = HeadController(
            self._sock_th.fileno(),
            required_module=head_type,
            msg_callback=self.toolhead_message_callback)

        self.head_resp_stack = [] if head_type == "USER" else None

    def on_bootstrap_toolhead(self, handler):
        self.toolhead.bootstrap(self.on_toolhead_ready)

    def on_clean_toolhead_error(self, handler):
        self.toolhead.errcode = 0

    def on_require_quit(self, handler):
        if self.buflen:
            raise InternalError(CMD_QUIT, MSG_OPERATION_ERROR)

        self.stack.exit_task(self)
        self.handler.send(packb((CMD_QUIT, 0)))

    def on_require_kill(self, handler):
        try:
            self.send_udps(0xfe)
            self.stack.exit_task(self)
        finally:
            from fluxmonitor.hal.tools import reset_mb
            reset_mb()
            self.handler.send(packb((CMD_QUIT, 0)))