From edbe977e43022de69bf875e825836aaf18280759 Mon Sep 17 00:00:00 2001 From: Luca Tessaro Date: Sun, 11 Oct 2020 14:26:53 +0200 Subject: [PATCH] Added emulator for fake serial: now is using 'realistic' timing for the acks to the feeder --- UIserver/hw_controller/device_serial.py | 12 ++-- UIserver/hw_controller/emulator.py | 84 +++++++++++++++++++++++++ UIserver/hw_controller/feeder.py | 19 +++++- 3 files changed, 107 insertions(+), 8 deletions(-) create mode 100644 UIserver/hw_controller/emulator.py diff --git a/UIserver/hw_controller/device_serial.py b/UIserver/hw_controller/device_serial.py index 271827a2..337aee1d 100644 --- a/UIserver/hw_controller/device_serial.py +++ b/UIserver/hw_controller/device_serial.py @@ -3,6 +3,7 @@ import time import sys import logging +from UIserver.hw_controller.emulator import Emulator # This class connect to a serial device # If the serial device request is not available it will create a virtual serial device @@ -16,6 +17,8 @@ def __init__(self, serialname = None, baudrate = None, logger_name = None): self.is_fake = False self._buffer = bytearray() self.echo = "" + self._emulator = Emulator() + try: args = dict( baudrate = self.baudrate, @@ -33,9 +36,7 @@ def __init__(self, serialname = None, baudrate = None, logger_name = None): def send(self, obj): if self.is_fake: - #print("Fake> " + str(obj)) - self.echo = obj - time.sleep(0.05) + self._emulator.send(obj) else: if self.serial.is_open: try: @@ -76,8 +77,5 @@ def readline(self): line = self.serial.readline() return line.decode(encoding='UTF-8') else: - if not self.echo == "": - echo = "ok" # sends "ok" as ack otherwise the feeder will stop sending buffered commands - self.echo = "" - return echo + return self._emulator.readline() return None diff --git a/UIserver/hw_controller/emulator.py b/UIserver/hw_controller/emulator.py new file mode 100644 index 00000000..eb530460 --- /dev/null +++ b/UIserver/hw_controller/emulator.py @@ -0,0 +1,84 @@ +import time, re, math +from collections import deque + +emulated_commands_with_delay = ["G0", "G00", "G1", "G01"] + +class Emulator(): + def __init__(self): + self.feedrate = 5000.0 + self.ack_buffer = deque() # used for the standard "ok" acks timing + self.message_buffer = deque() # used to emulate marlin response to special commands + self.last_time = time.time() + self.xr = re.compile("[X]([0-9.]+)($|\s)") + self.yr = re.compile("[Y]([0-9.]+)($|\s)") + self.fr = re.compile("[F]([0-9.]+)($|\s)") + self.last_x = 0.0 + self.last_y = 0.0 + + def get_x(self, line): + return float(self.xr.findall(line)[0][0]) + + def get_y(self, line): + return float(self.yr.findall(line)[0][0]) + + def _buffer_empty(self): + return len(self.ack_buffer)<1 + + def send(self, command): + if self._buffer_empty(): + self.last_time = time.time() + # TODO introduce the response for particular commands (like feedrate request, position request and others) + + # reset position for G28 command + if "G28" in command: + self.last_x = 0.0 + self.last_y = 0.0 + self.message_buffer.append("ok") + + # when receives a line calculate the time between the line received and when the ack must be sent back with the feedrate + if any(code in command for code in emulated_commands_with_delay): + # check if should update feedrate + f = self.fr.findall(command) + if len(f) > 0: + self.feedrate = float(f[0][0]) + + # get points coords + try: + x = self.get_x(command) + except: + x = self.last_x + try: + y = self.get_y(command) + except: + y = self.last_y + # calculate time + t = math.sqrt((x-self.last_x)**2 + (y-self.last_y)**2) / self.feedrate * 60.0 + if t == 0.0: + self.message_buffer.append("ok") + return + + # update positions + self.last_x = x + self.last_y = y + + # add calculated time + self.last_time += t + self.ack_buffer.append(self.last_time) + + else: + self.message_buffer.append("ok") + + def readline(self): + # special commands response + if len(self.message_buffer) >= 1: + return self.message_buffer.popleft() + + # standard lines acks (G0, G1) + if self._buffer_empty(): + return None + oldest = self.ack_buffer.popleft() + if oldest > time.time(): + self.ack_buffer.appendleft(oldest) + return None + else: + return "ok" \ No newline at end of file diff --git a/UIserver/hw_controller/feeder.py b/UIserver/hw_controller/feeder.py index 406d3a8d..1d0935c7 100644 --- a/UIserver/hw_controller/feeder.py +++ b/UIserver/hw_controller/feeder.py @@ -6,6 +6,7 @@ import json from collections import deque from copy import deepcopy +import re import logging from dotenv import load_dotenv @@ -79,6 +80,10 @@ def __init__(self, handler = None, **kargvs): self.serial = None self.line_number = 0 self._timeout_last_line = self.line_number + self.feedrate = 0 + + # commands parser + self.feed_regex = re.compile("[F]([0-9.]+)($|\s)") # buffer control attrs self.command_buffer = deque() @@ -114,6 +119,7 @@ def connect(self): # reset line number when connecting self.reset_line_number() + self.request_feedrate() # send the "on connection" script from the settings self.send_script(settings['scripts']['connection']) @@ -213,6 +219,7 @@ def _thf(self, code): # thread that keep reading the serial port def _thsr(self): + line = None while True: with self.serial_mutex: try: @@ -288,6 +295,10 @@ def parse_device_line(self, line): self.logger.error("No line was found for the number required. Restart numeration.") self.send_gcode_command("M110 N1") + # TODO check feedrate response for M220 and set feedrate + #elif "_______" in line: # must see the real output from marlin + # self.feedrate = .... # must see the real output from marlin + elif "echo:Unknown command:" in line: self.logger.error("Error: command not found. Can also be a communication error") @@ -334,6 +345,9 @@ def send_gcode_command(self, command): # check if the command is in the "BUFFERED_COMMANDS" list and stops if the buffer is full if any(code in command for code in BUFFERED_COMMANDS): + if "F" in command: + feed = self.feed_regex.findall(command) + self.feedrate = feed[0][0] with self.command_send_mutex: # wait until get some "ok" command to remove an element from the buffer pass @@ -355,7 +369,7 @@ def send_gcode_command(self, command): self._update_timeout() # update the timeout because a new command has been sent with self.command_buffer_mutex: - if(len(self.command_buffer)>=self.command_buffer_max_length): + if(len(self.command_buffer)>=self.command_buffer_max_length and not self.command_send_mutex.locked()): self.command_send_mutex.acquire() # if the buffer is full acquire the lock so that cannot send new lines until the reception of an ack. Notice that this will stop only buffered commands. The other commands will be sent anyway self.handler.on_new_line(line) # uses the handler callback for the new line @@ -371,6 +385,9 @@ def send_script(self, script): def reset_line_number(self, line_number = 2): self.logger.info("Resetting line number") self.send_gcode_command("M110 N{}".format(line_number)) + + def request_feedrate(self): + self.send_gcode_command("M220") def serial_ports_list(self): result = self.serial.serial_port_list()