guest_worker/sorteerhoed/plotter.py

176 lines
6.7 KiB
Python
Raw Normal View History

2019-10-23 10:56:28 +02:00
import queue
import logging
from pyaxidraw import axidraw
from queue import Queue
from threading import Event, Lock
2019-10-23 10:56:28 +02:00
from sorteerhoed.Signal import Signal
import time
class Plotter:
def __init__(self, config, eventQ: Queue, runningEvent: Event, scannerLock: Lock):
2019-10-30 12:44:25 +01:00
#TODO: scanningEvent -> CentralManagement.isScanning -> prevent plotter move during scan, failsafe
2019-10-23 10:56:28 +02:00
self.config = config
self.eventQ = eventQ
self.q = Queue()
self.isRunning = runningEvent
self.logger = logging.getLogger("sorteerhoed").getChild("plotter")
2019-10-31 13:55:22 +01:00
self.pen_down = False
2019-10-31 13:55:22 +01:00
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
2019-10-23 22:33:37 +02:00
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])
2019-10-23 10:56:28 +02:00
def start(self):
2019-10-31 13:55:22 +01:00
try:
if not self.config['dummy_plotter']:
self.ad = axidraw.AxiDraw()
2019-10-31 13:55:22 +01:00
self.ad.interactive()
# self.ad.plot_path()
2019-10-31 13:55:22 +01:00
connected = self.ad.connect()
if not connected:
raise Exception("Cannot connect to Axidraw")
2019-11-01 17:48:37 +01:00
# self.ad.options.units = 1 # set to use centimeters instead of inches
2019-10-23 22:33:37 +02:00
self.ad.options.accel = 100;
self.ad.options.speed_penup = 100
self.ad.options.speed_pendown = 100
2019-10-31 13:55:22 +01:00
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)
2019-10-31 13:55:22 +01:00
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")
2019-10-31 13:55:22 +01:00
# send shutdown signal (if not already set)
self.isRunning.clear()
2019-10-31 13:55:22 +01:00
def draw_segments(self, segments = []):
if not self.locked:
# acquire lock if not already done so
self.scannerLock.acquire()
self.locked = True
2019-10-31 13:55:22 +01:00
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
2019-10-31 13:55:22 +01:00
coordinates.append(coordinate)
2019-10-31 15:49:12 +01:00
self.logger.debug(f"Plot: {coordinates}")
2019-10-31 13:55:22 +01:00
if self.ad:
2019-11-01 17:48:37 +01:00
if len(coordinates) < 2:
2019-11-01 18:57:38 +01:00
self.logger.info("Plot single point (park?)")
2019-11-01 17:48:37 +01:00
self.ad.moveto(coordinates[0][0], coordinates[0][1])
else:
self.ad.plan_trajectory(coordinates)
2019-10-31 13:55:22 +01:00
# self.ad.moveto(move[0]* plotterWidth, move[1]*plotterHeight)
# self.logger.debug(f'handler! {move}')
pass
2019-10-31 13:55:22 +01:00
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)
2019-11-01 18:57:38 +01:00
#if self.goPark:
# print("seg",segment)
2019-10-31 13:55:22 +01:00
# 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:
2019-11-01 17:48:37 +01:00
if len(segments):
2019-10-31 13:55:22 +01:00
self.draw_segments(segments)
plotterRan = True
segments = [] #reset
2019-10-31 13:55:22 +01:00
# and change pen positions
self.setPenDown(segment[2] == 1)
2019-10-31 13:55:22 +01:00
segments.append(segment)
2019-10-31 13:55:22 +01:00
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
2019-10-31 13:55:22 +01:00
# else:
# time.sleep(.05)
# self.logger.debug(f'Plotter move: {move}')
self.logger.info("Stopping plotter")