def get_main_line(self):
blocks = self.get_blocks()
if len(blocks) == 0:
block = Block(self.img_width / 2, self.img_width / 2, min(1, self.img_height), min(1, self.img_height))
self.add_block(block)
return Line(block,
Block(self.img_width / 2, self.img_width / 2, 0, 0))
elif len(blocks) == 1:
return Line(blocks[0], Block(blocks[0].get_middle()[0],blocks[0].get_middle()[0],0,0))
else:
blocks = []
next_block = None
prev_block = Block(self.get_img_width() / 2, self.get_img_width() / 2, self.get_img_height(),
self.get_img_height())
for block in self.get_blocks():
if block != prev_block and (next_block is None or
(block.distance_from(prev_block) < next_block.distance_from(prev_block))):
next_block = block
blocks.append(next_block)
while True:
prev_block = blocks[-1]
blocks_in_range = []
for block in self.get_blocks():
if block.distance_from(prev_block) < Image.dist_threshold \
and block not in blocks and block.get_middle()[1] <= prev_block.get_middle()[1]: # TODO: lijn die terugkeert ondersteunen
blocks_in_range.append(block)
if len(blocks_in_range) == 0:
break
elif len(blocks)>1:
rico = get_rico(blocks[-2],blocks[-1])
else:
rico = -10000
smallest_dif = math.pi/2.2 #we do not want anything behind us
next_block = None
for block in blocks_in_range:
current_diff = Image.calculate_diff(math.atan(get_rico(prev_block, block)), math.atan(rico))
if current_diff < smallest_dif:
smallest_dif = current_diff
next_block = block
if next_block is None:
break
else:
blocks.append(next_block)
try:
x = (-blocks[-1].get_middle()[1])/get_rico(blocks[0],blocks[-1])+blocks[-1].get_middle()[0]
blocks.append(Block(x,x,0,0))
except ZeroDivisionError:
print get_rico(blocks[0],blocks[-1])
print blocks
print "-----------STOP---------------------------------------------------------------"
return Line(*blocks)
def is_crossing(main_line, blocks):
if len(blocks) >= 2:
for i in xrange(len(blocks) - 1):
for block2 in blocks[i + 1 :]:
difference = Image.Image.calculate_diff(
math.atan(lines.get_rico(blocks[i], block2)), math.atan(main_line.get_rico())
)
if math.radians(100) > difference > math.radians(80):
return True
return False
def go_first_block(power, line):
block = line.get_first_block()
location = block.get_middle()
width = block.get_image().get_img_width()
height = block.get_image().get_img_height()
mid_block = Block(width / 2, width / 2, height, height)
rico = lines.get_rico(block, mid_block)
radians = math.atan(rico)
if abs(radians) < math.pi / 4:
if radians > 0: # positief = naar links draaien
beeldverwerking_namespace.set_powers(0, 75)
else:
beeldverwerking_namespace.set_powers(75, 0)
else:
degrees = int(math.copysign(90, radians) - math.degrees(radians))
beeldverwerking_namespace.set_powers(60 - degrees / 4, 60 + degrees / 4)
def go_first_block_2(power, line):
block = line.get_first_block()
location = block.get_middle()
width = block.get_image().get_img_width()
height = block.get_image().get_img_height()
mid_block = Block(width / 2, width / 2, height, height)
rico = lines.get_rico(block, mid_block)
radians = math.atan(rico)
if abs(radians) < math.pi / 4 and location[1] < height - 7:
if radians > 0: # positief = naar links draaien
beeldverwerking_namespace.set_powers(0, 60)
else:
beeldverwerking_namespace.set_powers(60, 0)
else:
radians = math.atan(line.get_rico())
compensation = int(math.degrees(Image.Image.calculate_diff(math.pi, radians)))
if not width / 2 + 5 > location[0] > width / 2 - 5:
compensation -= location[0] - width / 2
beeldverwerking_namespace.set_powers(power - compensation / 4, power + compensation / 4)
