176 lines
6.7 KiB
Python
176 lines
6.7 KiB
Python
import queue
|
|
import logging
|
|
from pyaxidraw import axidraw
|
|
from queue import Queue
|
|
from threading import Event, Lock
|
|
from sorteerhoed.Signal import Signal
|
|
import time
|
|
|
|
class Plotter:
|
|
def __init__(self, config, eventQ: Queue, runningEvent: Event, scannerLock: Lock):
|
|
#TODO: scanningEvent -> CentralManagement.isScanning -> prevent plotter move during scan, failsafe
|
|
self.config = config
|
|
self.eventQ = eventQ
|
|
self.q = Queue()
|
|
self.isRunning = runningEvent
|
|
self.logger = logging.getLogger("sorteerhoed").getChild("plotter")
|
|
self.pen_down = False
|
|
|
|
self.plotWidth = self.config['scanner']['width'] / 10 / 2.54
|
|
self.plotHeight = self.config['scanner']['height'] / 10 / 2.54
|
|
self.xPadding = self.config['scanner']['left_padding'] / 10 / 2.54;
|
|
self.yPadding = self.config['scanner']['top_padding'] / 10 / 2.54;
|
|
self.logger.info(f"Paddings x: {self.xPadding} inch y: {self.yPadding} inch")
|
|
self.scannerLock = scannerLock
|
|
self.goPark = False
|
|
self.locked = False
|
|
self.ad = None
|
|
|
|
def park(self):
|
|
self.logger.info("Queue to park plotter")
|
|
self.goPark = True
|
|
#self.q.put([(1/2.54)/self.plotWidth + 1,0,0])
|
|
absPlotWidth = 26.5/2.54
|
|
topLeft = absPlotWidth / self.plotWidth #ignore changes in config plotwidth
|
|
self.q.put([(1/2.54)/absPlotWidth + topLeft,0,0])
|
|
|
|
|
|
def start(self):
|
|
try:
|
|
if not self.config['dummy_plotter']:
|
|
self.ad = axidraw.AxiDraw()
|
|
|
|
self.ad.interactive()
|
|
# self.ad.plot_path()
|
|
|
|
connected = self.ad.connect()
|
|
if not connected:
|
|
raise Exception("Cannot connect to Axidraw")
|
|
|
|
# self.ad.options.units = 1 # set to use centimeters instead of inches
|
|
self.ad.options.accel = 100;
|
|
self.ad.options.speed_penup = 100
|
|
self.ad.options.speed_pendown = 100
|
|
self.ad.options.model = 1 # 2 for A3, 1 for A4
|
|
self.ad.options.pen_pos_up = 100
|
|
|
|
self.park()
|
|
# self.ad.moveto(0,0)
|
|
|
|
else:
|
|
self.ad = None
|
|
self.axiDrawCueListener()
|
|
except Exception as e:
|
|
self.logger.exception(e)
|
|
finally:
|
|
self.logger.warning("Close Axidraw connection")
|
|
if self.ad:
|
|
try:
|
|
with self.scannerLock:
|
|
self.ad.moveto(0,0)
|
|
self.ad.disconnect()
|
|
except Exception as e:
|
|
self.logger.warning("Error on closing axidraw:")
|
|
self.logger.exception(e)
|
|
|
|
self.logger.info("Clear running Event")
|
|
# send shutdown signal (if not already set)
|
|
self.isRunning.clear()
|
|
|
|
def draw_segments(self, segments = []):
|
|
if not self.locked:
|
|
# acquire lock if not already done so
|
|
self.scannerLock.acquire()
|
|
self.locked = True
|
|
|
|
coordinates = []
|
|
for segment in segments:
|
|
coordinate = [
|
|
# mm to cm to inches
|
|
(segment[0]) * self.plotWidth,
|
|
(1-segment[1]) * self.plotHeight
|
|
]
|
|
#prevent drawing when not in drwaing area
|
|
# this is a failsafe for a malicious working or glitching script, as this should also be done in the javascript
|
|
if self.pen_down:
|
|
if coordinate[0] < self.xPadding or coordinate[0] > self.xPadding+self.plotWidth or \
|
|
coordinate[1] < self.yPadding or coordinate[1] > self.yPadding + self.plotHeight:
|
|
self.logger.warn(f"Skip drawing for: {coordinates} out of bounds")
|
|
continue
|
|
|
|
coordinates.append(coordinate)
|
|
self.logger.debug(f"Plot: {coordinates}")
|
|
if self.ad:
|
|
if len(coordinates) < 2:
|
|
self.logger.info("Plot single point (park?)")
|
|
self.ad.moveto(coordinates[0][0], coordinates[0][1])
|
|
else:
|
|
self.ad.plan_trajectory(coordinates)
|
|
# self.ad.moveto(move[0]* plotterWidth, move[1]*plotterHeight)
|
|
# self.logger.debug(f'handler! {move}')
|
|
pass
|
|
|
|
def setPenDown(self, pen_state):
|
|
"""
|
|
False: pen raised, True: pen_lower
|
|
"""
|
|
if pen_state != self.pen_down:
|
|
self.pen_down = pen_state
|
|
self.logger.info("Changed pen: {}".format('down' if pen_state else 'up'))
|
|
if self.ad:
|
|
if pen_state:
|
|
self.ad.pen_lower()
|
|
else:
|
|
self.ad.pen_raise()
|
|
return True
|
|
return False
|
|
|
|
def axiDrawCueListener(self):
|
|
plotterRan = False
|
|
segments = []
|
|
while self.isRunning.is_set():
|
|
# TODO: queue that collects a part of the path data
|
|
# on a specific limit or on a specific time interval, do the plot
|
|
# also, changing ad.pen_raise() or ad.pen_lower() trigger a new segment
|
|
# Plot with ad.plan_trajectory() ??
|
|
try:
|
|
# if no info comes in for .5sec, plot that segment
|
|
segment = self.q.get(True, .5)
|
|
|
|
#if self.goPark:
|
|
# print("seg",segment)
|
|
|
|
# change of pen state? draw previous segments!
|
|
if (segment[2] == 1 and not self.pen_down) or (segment[2] == 0 and self.pen_down) or len(segments) > 150:
|
|
if len(segments):
|
|
self.draw_segments(segments)
|
|
plotterRan = True
|
|
segments = [] #reset
|
|
|
|
# and change pen positions
|
|
self.setPenDown(segment[2] == 1)
|
|
|
|
|
|
segments.append(segment)
|
|
|
|
except queue.Empty as e:
|
|
self.logger.debug("Timeout queue.")
|
|
if len(segments):
|
|
# segments to plot!
|
|
self.draw_segments(segments)
|
|
plotterRan = True
|
|
segments = []
|
|
elif plotterRan:
|
|
plotterRan = False
|
|
self.eventQ.put(Signal('plotter.finished'))
|
|
if self.goPark:
|
|
self.eventQ.put(Signal('plotter.parked'))
|
|
if self.locked:
|
|
self.scannerLock.release()
|
|
self.locked = False
|
|
self.goPark = False
|
|
# else:
|
|
# time.sleep(.05)
|
|
# self.logger.debug(f'Plotter move: {move}')
|
|
self.logger.info("Stopping plotter")
|