161 lines
7.4 KiB
Python
161 lines
7.4 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"
|
|
|
|
#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 , 'A', 0 , 3, 19.11, 180.40, 198.40, 189.40, 19.11 , 59.06 , 170.46 , 64.95 , 187.47 ),
|
|
(4 , 'A', 0 , 4, 40.89, 180.40, 198.40, 189.40, 40.89 , 118.09 , 136.38 , 129.87 , 149.98 ),
|
|
(5 , 'A', 0 , 5, 30.00, 111.90, 129.90, 120.90, 30.00 , 55.95 , 96.91 , 64.95 , 112.50 ),
|
|
(6 , 'B', 60 , 1, 0.00 , 57.00 , 75.00 , 66.00 , 60.00 , 49.36 , 28.50 , 64.95 , 37.50 ),
|
|
(7 , 'B', 60 , 2, 0.00 , 132.00, 150.00, 141.00, 60.00 , 114.32 , 66.00 , 129.90 , 75.00 ),
|
|
(8 , 'B', 60 , 3, 19.11, 180.40, 198.40, 189.40, 79.11 , 177.15 , 34.08 , 194.83 , 37.48 ),
|
|
(9 , 'B', 60 , 4, 40.89, 180.40, 198.40, 189.40, 100.89, 177.15 , -34.08 , 194.83 , -37.48 ),
|
|
(10, 'B', 60 , 5, 30.00, 111.90, 129.90, 120.90, 90.00 , 111.90 , 0.00 , 129.90 , 0.00 ),
|
|
(11, 'C', 120, 1, 0.00 , 57.00 , 75.00 , 66.00 , 120.00, 49.36 , -28.50 , 64.95 , -37.50 ),
|
|
(12, 'C', 120, 2, 0.00 , 132.00, 150.00, 141.00, 120.00, 114.32 , -66.00 , 129.90 , -75.00 ),
|
|
(13, 'C', 120, 3, 19.11, 180.40, 198.40, 189.40, 139.11, 118.09 , -136.38, 129.87 , -149.98),
|
|
(14, 'C', 120, 4, 40.89, 180.40, 198.40, 189.40, 160.89, 59.06 , -170.46, 64.95 , -187.47),
|
|
(15, 'C', 120, 5, 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, 'D', 180, 3, 19.11, 180.40, 198.40, 189.40, 199.11, -59.06 , -170.46, -64.95 , -187.47),
|
|
(19, 'D', 180, 4, 40.89, 180.40, 198.40, 189.40, 220.89, -118.09, -136.38, -129.87, -149.98),
|
|
(20, 'D', 180, 5, 30.00, 111.90, 129.90, 120.90, 210.00, -55.95 , -96.91 , -64.95 , -112.50),
|
|
(21, 'E', 240, 1, 0.00 , 57.00 , 75.00 , 66.00 , 240.00, -49.36 , -28.50 , -64.95 , -37.50 ),
|
|
(22, 'E', 240, 2, 0.00 , 132.00, 150.00, 141.00, 240.00, -114.32, -66.00 , -129.90, -75.00 ),
|
|
(23, 'E', 240, 3, 19.11, 180.40, 198.40, 189.40, 259.11, -177.15, -34.08 , -194.83, -37.48 ),
|
|
(24, 'E', 240, 4, 40.89, 180.40, 198.40, 189.40, 280.89, -177.15, 34.08 , -194.83, 37.48 ),
|
|
(25, 'E', 240, 5, 30.00, 111.90, 129.90, 120.90, 270.00, -111.90, 0.00 , -129.90, 0.00 ),
|
|
(26, 'F', 300, 1, 0.00 , 57.00 , 75.00 , 66.00 , 300.00, -49.36 , 28.50 , -64.95 , 37.50 ),
|
|
(27, 'F', 300, 2, 0.00 , 132.00, 150.00, 141.00, 300.00, -114.32, 66.00 , -129.90, 75.00 ),
|
|
(28, 'F', 300, 3, 19.11, 180.40, 198.40, 189.40, 319.11, -118.09, 136.38 , -129.87, 149.98 ),
|
|
(29, 'F', 300, 4, 40.89, 180.40, 198.40, 189.40, 340.89, -59.06 , 170.46 , -64.95 , 187.47 ),
|
|
(30, 'F', 300, 5, 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 = None
|
|
|
|
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 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_pucks(point_list, block_id=None):
|
|
for puck in get_pucks(id):
|
|
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
|
|
|
|
detect_pucks(points)
|
|
for puck in get_pucks():
|
|
print puck.id, puck.detect
|