class DurarararaControlTest(SharedTestCase):
def setUp(self):
SharedTestCase.setUp(self)
self.t = HeadController(self.rsock.fileno())
def test_durararara(self):
self.t.bootstrap()
self.t.send_timestamp = 0
self.assertFalse(self.t.ready)
self.t.patrol()
self.assertTrue(self.t.ready)
self.assertRaises(RuntimeError, self.send_and_process, PONG_MSG_ER4)
self.assertFalse(self.t.ready)
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 ExtruderGeneralTest(SharedTestCase):
def setUp(self):
SharedTestCase.setUp(self)
self.t = HeadController(self.rsock.fileno(),
required_module="EXTRUDER")
def _bootstrap(self):
def callback(caller):
self.callback_log.append(("boot", callback))
self.callback_log = []
self.t.bootstrap(callback)
self.assertRecv(HELLO_CMD)
self.send_and_process(EXTRUDER_HELLO_RESP)
self.assertEqual(self.callback_log, [("boot", callback)])
self.assertTrue(self.t.sendable())
def _recover(self, cmd_queue, resp_queue):
def callback(caller):
self.callback_log.append(("recover", callback))
self.callback_log = []
self.t.recover(callback)
while cmd_queue:
self.assertEqual(self.callback_log, [])
cmd = cmd_queue.pop(0)
self.assertRecv(cmd)
self.send_and_process(resp_queue.pop(0))
self.assertEqual(self.callback_log, [("recover", callback)])
def _standby(self, cmd_queue, resp_queue):
def callback(caller):
self.callback_log.append(("standby", callback))
self.callback_log = []
self.t.standby(callback)
while cmd_queue:
self.assertEqual(self.callback_log, [])
cmd = cmd_queue.pop(0)
self.assertRecv(cmd)
self.send_and_process(resp_queue.pop(0))
self.assertEqual(self.callback_log, [("standby", callback)])
def _respcmd(self, recv_cmd, resp):
def callback(caller):
self.callback_log.append(("respcmd", callback))
self.callback_log = []
self.t.set_command_callback(callback)
self.assertRecv(recv_cmd)
self.send_and_process(resp)
self.assertTrue(self.t.sendable())
self.assertEqual(self.callback_log, [("respcmd", callback)])
def test_bootstrap_recover_standby(self):
self.assertRaises(RuntimeError, self.t.ext.set_heater, 0, 100)
self._bootstrap()
self.assertTrue(self.t.sendable())
self.assertEqual(self.t.profile, EXTRUDER_HELLO_RESP_DICT)
self.t.ext.set_heater(0, 200)
self.assertRecv(HEAT_0_200_CMD)
self.send_and_process(HEAT_OK)
self.assertTrue(self.t.sendable())
self._bootstrap()
self._recover(cmd_queue=[HEAT_0_200_CMD], resp_queue=[HEAT_OK])
self._standby(cmd_queue=[FAN_0_0, HEAT_0_0_CMD],
resp_queue=[FAN_OK, HEAT_OK])
self._recover(cmd_queue=[HEAT_0_200_CMD], resp_queue=[HEAT_OK])
def test_ping_pong(self):
self._bootstrap()
self.t.patrol()
self.assertRecv(PING_CMD)
self.send_and_process(PONG_EXR["TNANR40"])
self.assertEqual(self.t.status, ST_DICT["TNANR40"])
self.send_and_process(PONG_EXR["T0R210"])
self.assertEqual(self.t.status, ST_DICT["T0R210"])
self.send_and_process(PONG_EXR["T0R40"])
self.assertEqual(self.t.status, ST_DICT["T0R40"])
self.send_and_process(PONG_EXR["T0R210"])
self.assertEqual(self.t.status, ST_DICT["T0R210"])
def test_hello_offline(self):
self.t.bootstrap(None)
self.assertRecv(HELLO_CMD)
retry = 3
for i in range(retry):
self.t.send_timestamp = 0
self.t.patrol()
self.assertRecv(HELLO_CMD)
self.t.send_timestamp = 0
self.assertRaises(RuntimeError, self.t.patrol)
self.assertFalse(self.t.sendable())
def test_ping_offline(self):
self._bootstrap()
self.t.patrol()
self.assertRecv(PING_CMD)
retry = 3
for i in range(retry):
self.t.send_timestamp = 0
self.t.patrol()
self.assertRecv(PING_CMD)
self.t.send_timestamp = 0
self.assertRaises(RuntimeError, self.t.patrol)
self.assertFalse(self.t.sendable())
self.assertRaises(RuntimeError, self.t.ext.set_heater, 0, 100)
def test_error_code(self):
self._bootstrap()
self.assertEqual(self.t.error_code, 0)
self.t.patrol()
self.assertRecv(PING_CMD)
self.send_and_process(PONG_EXR["ER8"])
self.assertEqual(self.t.error_code, 8)
self.assertTrue(self.t.sendable())
def test_command_callback(self):
self._bootstrap()
self.t.ext.set_heater(0, 200)
self._respcmd(HEAT_0_200_CMD, HEAT_OK)
def test_allset(self):
self._bootstrap()
def callback(caller):
self.callback_log.append(("allset", callback))
# allset=False, Update TT:nan, RT:40.0
self.t.patrol()
self.assertRecv(PING_CMD)
self.send_and_process(PONG_EXR["TNANR40"])
self.assertTrue(self.t.ext.allset())
self.t.set_allset_callback(callback)
# Update TT: 210
self.t.ext.set_heater(0, 210)
self._respcmd(HEAT_0_210_CMD, HEAT_OK)
self.callback_log = []
self.assertFalse(self.t.ext.allset())
# allset=False, Update TT:210, RT:40.0
self.t.lastupdate = 0
self.t.patrol()
self.assertRecv(PING_CMD)
self.send_and_process(PONG_EXR["T210R40"])
self.assertFalse(self.t.ext.allset())
self.assertEqual(self.callback_log, [])
# allset=True, Update TT:210, RT:210.0
self.send_and_process(PONG_EXR["T210R210"])
self.assertTrue(self.t.ext.allset())
self.assertEqual(self.callback_log, [("allset", callback)])
def test_cmd_timeout(self):
self._bootstrap()
self.t.ext.set_heater(0, 210)
self.assertRecv(HEAT_0_210_CMD)
for i in range(3):
self.t.send_timestamp = 0
self.t.patrol()
self.assertRecv(HEAT_0_210_CMD)
self.t.send_timestamp = 0
self.assertRaises(RuntimeError, self.t.patrol)
self.assertFalse(self.t.sendable())
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)))