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)
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", "")
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", "")
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)))