Added emulator for fake serial: now is using 'realistic' timing for t…

…he acks to the feeder

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

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"

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()