293 lines
12 KiB
Python
293 lines
12 KiB
Python
###################################################################################################
|
|
#DEFINITIONS
|
|
###################################################################################################
|
|
|
|
PLATE_SIZE = 480
|
|
BLOCK_ROI_TOLERANCE = 12 #mm
|
|
LED_TOLERANCE = 8 #mm Distance between LEDs = 18mm
|
|
PUCK_SIZE = 65
|
|
|
|
DET_UNIPUCK = "unipuck"
|
|
DET_MINISPINE = "minispine"
|
|
DET_ERROR = "error"
|
|
DET_EMPTY = "empty"
|
|
DET_UNKNOWN = "unknown"
|
|
|
|
BLOCKS = ('A', 'B', 'C', 'D', 'E', 'F')
|
|
|
|
#Layout table
|
|
puck_layout = (
|
|
#Num Elm A0 Index A1 Uni Mini Center Angle Xuni Yuni Xmini=Xc Ymini==Yc
|
|
(1 , 'A', 0 , 1, 0.00 , 57.00 , 75.00 , 66.00 , 0.00 , 0.00 , 57.00 , 0.00 , 75.00 ),
|
|
(2 , 'A', 0 , 2, 0.00 , 132.00, 150.00, 141.00, 0.00 , 0.00 , 132.00 , 0.00 , 150.00 ),
|
|
(3 , 'F', 0 , 5, 19.11, 180.40, 198.40, 189.40, 19.11 , 59.06 , 170.46 , 64.95 , 187.47 ),
|
|
(4 , 'F', 0 , 4, 40.89, 180.40, 198.40, 189.40, 40.89 , 118.09 , 136.38 , 129.87 , 149.98 ),
|
|
(5 , 'F', 0 , 3, 30.00, 111.90, 129.90, 120.90, 30.00 , 55.95 , 96.91 , 64.95 , 112.50 ),
|
|
(6 , 'F', 60 , 1, 0.00 , 57.00 , 75.00 , 66.00 , 60.00 , 49.36 , 28.50 , 64.95 , 37.50 ),
|
|
(7 , 'F', 60 , 2, 0.00 , 132.00, 150.00, 141.00, 60.00 , 114.32 , 66.00 , 129.90 , 75.00 ),
|
|
(8 , 'E', 60 , 5, 19.11, 180.40, 198.40, 189.40, 79.11 , 177.15 , 34.08 , 194.83 , 37.48 ),
|
|
(9 , 'E', 60 , 4, 40.89, 180.40, 198.40, 189.40, 100.89, 177.15 , -34.08 , 194.83 , -37.48 ),
|
|
(10, 'E', 60 , 3, 30.00, 111.90, 129.90, 120.90, 90.00 , 111.90 , 0.00 , 129.90 , 0.00 ),
|
|
(11, 'E', 120, 1, 0.00 , 57.00 , 75.00 , 66.00 , 120.00, 49.36 , -28.50 , 64.95 , -37.50 ),
|
|
(12, 'E', 120, 2, 0.00 , 132.00, 150.00, 141.00, 120.00, 114.32 , -66.00 , 129.90 , -75.00 ),
|
|
(13, 'D', 120, 5, 19.11, 180.40, 198.40, 189.40, 139.11, 118.09 , -136.38, 129.87 , -149.98),
|
|
(14, 'D', 120, 4, 40.89, 180.40, 198.40, 189.40, 160.89, 59.06 , -170.46, 64.95 , -187.47),
|
|
(15, 'D', 120, 3, 30.00, 111.90, 129.90, 120.90, 150.00, 55.95 , -96.91 , 64.95 , -112.50),
|
|
(16, 'D', 180, 1, 0.00 , 57.00 , 75.00 , 66.00 , 180.00, 0.00 , -57.00 , 0.00 , -75.00 ),
|
|
(17, 'D', 180, 2, 0.00 , 132.00, 150.00, 141.00, 180.00, 0.00 , -132.00, 0.00 , -150.00),
|
|
(18, 'C', 180, 5, 19.11, 180.40, 198.40, 189.40, 199.11, -59.06 , -170.46, -64.95 , -187.47),
|
|
(19, 'C', 180, 4, 40.89, 180.40, 198.40, 189.40, 220.89, -118.09, -136.38, -129.87, -149.98),
|
|
(20, 'C', 180, 3, 30.00, 111.90, 129.90, 120.90, 210.00, -55.95 , -96.91 , -64.95 , -112.50),
|
|
(21, 'C', 240, 1, 0.00 , 57.00 , 75.00 , 66.00 , 240.00, -49.36 , -28.50 , -64.95 , -37.50 ),
|
|
(22, 'C', 240, 2, 0.00 , 132.00, 150.00, 141.00, 240.00, -114.32, -66.00 , -129.90, -75.00 ),
|
|
(23, 'B', 240, 5, 19.11, 180.40, 198.40, 189.40, 259.11, -177.15, -34.08 , -194.83, -37.48 ),
|
|
(24, 'B', 240, 4, 40.89, 180.40, 198.40, 189.40, 280.89, -177.15, 34.08 , -194.83, 37.48 ),
|
|
(25, 'B', 240, 3, 30.00, 111.90, 129.90, 120.90, 270.00, -111.90, 0.00 , -129.90, 0.00 ),
|
|
(26, 'B', 300, 1, 0.00 , 57.00 , 75.00 , 66.00 , 300.00, -49.36 , 28.50 , -64.95 , 37.50 ),
|
|
(27, 'B', 300, 2, 0.00 , 132.00, 150.00, 141.00, 300.00, -114.32, 66.00 , -129.90, 75.00 ),
|
|
(28, 'A', 300, 5, 19.11, 180.40, 198.40, 189.40, 319.11, -118.09, 136.38 , -129.87, 149.98 ),
|
|
(29, 'A', 300, 4, 40.89, 180.40, 198.40, 189.40, 340.89, -59.06 , 170.46 , -64.95 , 187.47 ),
|
|
(30, 'A', 300, 3, 30.00, 111.90, 129.90, 120.90, 330.00, -55.95 , 96.91 , -64.95 , 112.5 ),
|
|
)
|
|
|
|
###################################################################################################
|
|
#Puck class
|
|
###################################################################################################
|
|
class Puck:
|
|
def __init__(self, id, block, index, angle, center, led_uni, led_mini):
|
|
self.id = id
|
|
self.block = block
|
|
self.index = index
|
|
self.angle = angle
|
|
self.center = center
|
|
self.led_uni = led_uni
|
|
self.led_mini = led_mini
|
|
self.detect = DET_UNKNOWN
|
|
|
|
def __str__(self):
|
|
return "Number: " + str(self.id) + "\nBlock: " + str(self.block) + "\nIndex: " + str(self.index) + "\nAngle: " + str(self.angle) + \
|
|
"\nCenter: " + str(self.center) + "\nLed Unipuck: " + str(self.led_uni) + "\nLed Minispine: " + str(self.led_mini)
|
|
|
|
def get_name(self):
|
|
return str(self.block) + str(self.index)
|
|
|
|
def match(self, x, y):
|
|
if math.hypot(x-self.led_uni[0], y-self.led_uni[1]) <= LED_TOLERANCE:
|
|
return DET_UNIPUCK
|
|
if math.hypot(x-self.led_mini[0], y-self.led_mini[1]) <= LED_TOLERANCE:
|
|
return DET_MINISPINE
|
|
return None
|
|
|
|
|
|
|
|
_block_ids = []
|
|
_puck_list = []
|
|
_block_list = []
|
|
|
|
for p in(puck_layout):
|
|
puck = Puck(p[0], p[1], p[3], p[8], (p[11],p[12]), (p[9],p[10]), (p[11],p[12]))
|
|
_puck_list.append(puck)
|
|
if puck.block not in (_block_ids):
|
|
_block_ids.append(puck.block)
|
|
|
|
def get_puck(id):
|
|
for p in _puck_list:
|
|
if id==p.id:
|
|
return p
|
|
return None
|
|
|
|
def get_pucks(block = None):
|
|
ret = []
|
|
for p in _puck_list:
|
|
if (block is None) or (block==p.block):
|
|
ret.append(p)
|
|
return ret
|
|
|
|
|
|
###################################################################################################
|
|
#Block class
|
|
###################################################################################################
|
|
|
|
class Block:
|
|
def __init__(self, id, angle_range, x_range, y_range):
|
|
self.id = id
|
|
self.angle_range = angle_range
|
|
self.x_range = x_range
|
|
self.y_range = y_range
|
|
self.roi = (self.x_range[0] - BLOCK_ROI_TOLERANCE, self.y_range[0] - BLOCK_ROI_TOLERANCE,
|
|
self.x_range[1] + BLOCK_ROI_TOLERANCE, self.y_range[1] + BLOCK_ROI_TOLERANCE)
|
|
|
|
def __str__(self):
|
|
return "Id: " + str(self.id) + "\nAngle: " + str(self.angle_range) + "\nX: " + str(self.x_range) + "\nY: " + str(self.y_range)
|
|
|
|
|
|
for id in _block_ids:
|
|
pucks = get_pucks(id)
|
|
angles, x, y = [], [], []
|
|
for p in pucks:
|
|
angles.append(p.angle)
|
|
x.append(p.led_uni[0])
|
|
x.append(p.led_mini[0])
|
|
y.append(p.led_uni[1])
|
|
y.append(p.led_mini[1])
|
|
el = Block(id,(min(angles), max(angles)), (min(x), max(x)), (min(y), max(y)))
|
|
_block_list.append(el)
|
|
|
|
def get_block(id):
|
|
for e in _block_list:
|
|
if id==e.id:
|
|
return e
|
|
return None
|
|
|
|
def get_blocks():
|
|
return _block_list
|
|
|
|
|
|
|
|
|
|
###################################################################################################
|
|
#Detection utilities
|
|
###################################################################################################
|
|
|
|
|
|
def _detect_puck(point_list, puck):
|
|
puck.detect = DET_ERROR
|
|
for point in point_list:
|
|
match = puck.match(point[0], point[1])
|
|
if match is not None:
|
|
if match==DET_UNIPUCK:
|
|
puck.detect = DET_EMPTY if (puck.detect==DET_MINISPINE) else DET_UNIPUCK
|
|
elif match==DET_MINISPINE:
|
|
puck.detect = DET_EMPTY if (puck.detect==DET_UNIPUCK) else DET_MINISPINE
|
|
|
|
def detect_pucks(point_list, id=None):
|
|
if (id is None) or (id in BLOCKS):
|
|
for puck in get_pucks(id):
|
|
_detect_puck(point_list, puck)
|
|
else:
|
|
puck = get_puck(int(id))
|
|
print puck
|
|
_detect_puck(point_list, puck)
|
|
|
|
|
|
def clear_detection(block_id=None):
|
|
for puck in get_pucks(block_id):
|
|
puck.detect = DET_UNKNOWN
|
|
return get_puck_detection_dict(block_id)
|
|
|
|
|
|
def get_puck_detection(det_type, block_id=None):
|
|
ret = []
|
|
for puck in get_pucks(block_id):
|
|
if puck.detect == det_type:
|
|
ret.append(puck)
|
|
return ret
|
|
|
|
def get_unipucks(block_id=None):
|
|
return get_puck_detection(DET_UNIPUCK, block_id)
|
|
|
|
def get_minispines(block_id=None):
|
|
return get_puck_detection(DET_MINISPINE, block_id)
|
|
|
|
def get_empties(block_id=None):
|
|
return get_puck_detection(DET_EMPTY, block_id)
|
|
|
|
def get_unknowns(block_id=None):
|
|
return get_puck_detection(DET_UNKNOWN, block_id)
|
|
|
|
def get_det_errors(block_id=None):
|
|
return get_puck_detection(DET_ERROR, block_id)
|
|
|
|
def get_puck_detection_dict(block_id):
|
|
ret = {}
|
|
pucks = []
|
|
for puck in get_unipucks(block_id):
|
|
pucks.append(puck.get_name())
|
|
pucks.sort()
|
|
ret["Unipuck"] = pucks
|
|
pucks = []
|
|
for puck in get_minispines(block_id):
|
|
pucks.append(puck.get_name())
|
|
pucks.sort()
|
|
ret["Minispine"] = pucks
|
|
pucks = []
|
|
for puck in get_det_errors(block_id):
|
|
pucks.append(puck.get_name())
|
|
pucks.sort()
|
|
ret["Error"] = pucks
|
|
pucks = []
|
|
for puck in get_empties(block_id):
|
|
pucks.append(puck.get_name())
|
|
pucks.sort()
|
|
ret["Empty"] = pucks
|
|
pucks = []
|
|
for puck in get_unknowns(block_id):
|
|
pucks.append(puck.get_name())
|
|
pucks.sort()
|
|
ret["Unknown"] = pucks
|
|
return ret
|
|
|
|
###################################################################################################
|
|
#Plotting
|
|
###################################################################################################
|
|
|
|
from plotutils import *
|
|
|
|
def plot_base_plate(points = None, show_detect = True, title = None, p = None):
|
|
colors = (Color.RED, Color.BLUE, Color.MAGENTA, Color(128,0,128), Color(0,128,0), Color(255,128,0))
|
|
if p is None: p = plot(None, title=title)[0]
|
|
p.getAxis(p.AxisId.Y).setInverted(True)
|
|
plot_circle(p, 0, 0, PLATE_SIZE/2, width = 0, color = Color.GRAY, name = "Plate")
|
|
plot_point(p, 0, 0, size = 10, color = Color.GRAY, name = "Center")
|
|
#p.setLegendVisible(True)
|
|
for block in get_blocks():
|
|
(xmin, xmax) = block.x_range
|
|
(ymin, ymax) = block.y_range
|
|
(xmin, ymin, xmax, ymax ) = block.roi
|
|
index = get_blocks().index(block)
|
|
r = plot_rectangle(p, xmin, ymin, xmax, ymax, width =0, color=colors[index], name = block.id)
|
|
#In the first time the plot shows, it takes some time for the color to be assigned
|
|
#while r.color is None:
|
|
# time.sleep(0.001)
|
|
|
|
if block.id in ('A', 'F'):
|
|
x, y = (xmin + xmax)/2, ymax + 5
|
|
elif block.id in ('C', 'D'):
|
|
x, y = (xmin + xmax)/2, ymin - 5
|
|
elif block.id == 'B':
|
|
x, y = xmax + 5, (ymin + ymax)/2
|
|
elif block.id == 'E':
|
|
x, y = xmin - 5, (ymin + ymax)/2
|
|
|
|
p.addText(x,y, str(block.id), r.color)
|
|
|
|
for puck in get_pucks(block.id):
|
|
(xu, yu) = puck.led_uni
|
|
(xm, ym) = puck.led_mini
|
|
plot_point(p, xu, yu, size = 3, color = r.color, name = str(puck.id)+"u")
|
|
plot_point(p, xm, ym, size = 7, color = r.color, name = str(puck.id)+"m")
|
|
plot_circle(p, xu, yu, LED_TOLERANCE, width = 0, color = r.color, name = str(puck.id)+"uc")
|
|
plot_circle(p, xm, ym, LED_TOLERANCE, width = 0, color = r.color, name = str(puck.id)+"mc")
|
|
p.addText((xu+xm)/2, (yu+ym)/2, str(puck.id), r.color)
|
|
c,w = Color.GRAY,0
|
|
if show_detect:
|
|
if puck.detect == DET_UNIPUCK:
|
|
c,w = Color.BLACK,1
|
|
elif puck.detect == DET_MINISPINE:
|
|
c,w = Color(150, 100, 50),1
|
|
elif puck.detect == DET_ERROR:
|
|
c = Color(128,0,0)
|
|
plot_circle(p, xm, ym, PUCK_SIZE/2, width = w, color = c , name = str(puck.id))
|
|
if points is not None:
|
|
for point in points:
|
|
c, w = Color.GRAY, 1
|
|
for puck in get_pucks():
|
|
match = puck.match(point[0], point[1])
|
|
if match is not None:
|
|
w=2
|
|
c = Color.DARK_GRAY if match == "minispine" else Color.BLACK
|
|
|
|
|
|
plot_cross(p,point[0], point[1], size = 12, width = w, color = c, name = "P"+ str(points.index(point)))
|
|
#plot_point(p,point[0], point[1], size = 5, color = Color.BLACK, name = "Pc"+ str(points.index(point)))
|