def on_first_run(self, desire, *args, **kwargs): subtasks = [] increment = 20 deadband = 25 current = shm.kalman.heading.get() while abs(heading_sub_degrees(current, desire)) % 360 > deadband: current -= math.copysign(increment, heading_sub_degrees(current, desire)) subtasks.append(Heading(current)) self.task = Sequential(subtasks=subtasks)
def on_run(self, target, *args, **kwargs): current = shm.kalman.heading.get() diff = heading_sub_degrees(target, current) inc = [-1, 1][diff > 0] * 12 RelativeToCurrentHeading(inc)() if abs(diff) < 5: self.finish()
def on_run(self, n): new_roll = shm.kalman.roll.get() self.cumul += heading_sub_degrees(new_roll, self.roll) self.roll = new_roll if abs(self.cumul) > 360 * n: self.finish()
def on_run(self, desire, *args, **kwargs): current = shm.kalman.heading.get() diff = heading_sub_degrees(desire, current) relative = math.copysign(self.relative_desire, diff) RelativeToCurrentHeading(relative)() if abs(diff) < self.deadband: self.finish()
def on_run(self, n): new_heading = shm.kalman.heading.get() self.cumul += heading_sub_degrees(new_heading, self.heading) self.heading = new_heading if abs(self.cumul) > 360 * n: self.finish()
def find_closest_angle(self, target, *angles, post=False): if len(angles) == 0: return # print(angles) closest = min(angles, key=lambda x: abs_heading_sub_degrees(target, x)) print(closest) if post: shm.bins_garlic.angle_offset.set(heading_sub_degrees(target, closest)) return closest
def on_run(self, desire, relative_desire, relative_deadband, *args, **kwargs): current = shm.kalman.heading.get() diff = heading_sub_degrees(call_if_function(desire), current) if abs(diff) < relative_deadband: self.finish() return relative = math.copysign(relative_desire, diff) self.rel_curr_heading(relative)
def is_done(heading_vect, heading, pos, trg, dst, deadband, abs_offset): v = (np.float64([heading_vect()]).view(np.complex128) * -dst).view( np.float64)[0] + abs_offset pos_var = pos() #print(shm.bins_status.cover_x.get(), shm.bins_status.cover_y.get(), v) #print(pos_var) print(pos_var - v, trg, heading()) vv = abs(heading_sub_degrees(trg, heading(), math.pi*2)) < math.radians(5) and \ abs(pos_var[0] - v[0]) < deadband and \ abs(pos_var[1] - v[1]) < deadband #print(vv) return vv
def find_crucifix_angles(self, garlic_mask): lines = find_lines(garlic_mask, 2, pi / 180, self.options['garlic_line_threshold'])[0] # print(lines) center = garlic_mask.shape[0] // 2, garlic_mask.shape[1] // 2 def distance_from_center(line): num = abs((line[3] - line[1]) * center[1] - (line[2] - line[0]) * center[0] + line[2] * line[1] - line[3] * line[0]) denom = sqrt((line[3] - line[1])**2 + (line[2] - line[0])**2) return num / denom lines = list(filter(lambda x: distance_from_center(x) < 20, lines)) angles = np.array([lines_to_angles(l) * 180 / pi for l in lines], dtype=np.float32) average = average_headings_degrees(angles) + 90 # print(average) shm.recovery_crucifix.angle_offset.set( heading_sub_degrees(self.options['manipulator_angle'], average)) return lines, average
def find_vampire(self, mat, split, color, distance): # mask = thresh_color_distance(split, color, distance) mask, _ = thresh_color_distance(split, color, distance, weights=[0.5, 2, 2]) self.post('purple', mask) rects = self.intersect_rectangles(self.rectangles, mask, self.options['intersection_size_min']) if self.rectangles: empty = max([r['rectangle'] for r in self.rectangles], key=lambda r: r[1][0] * r[1][1]) align_angle = empty[2] + 90 if empty[1][1] > empty[1][0] else empty[2] align_angle = 360 + align_angle if align_angle < 0 else align_angle shm.recovery_vampire.empty_visible.set(True) shm.recovery_vampire.empty_x.set(int(empty[0][0])) shm.recovery_vampire.empty_y.set(int(empty[0][1])) shm.recovery_vampire.empty_offset_x.set(int(empty[0][0] + min(empty[1]) * self.options['open_offset'])) shm.recovery_vampire.empty_offset_y.set(int(empty[0][1])) shm.recovery_vampire.empty_angle_offset.set(heading_sub_degrees(self.options['manipulator_angle'], align_angle)) shm.recovery_vampire.empty_size.set(empty[1][0] * empty[1][1]) cv2.circle(mat, (int(empty[0][0]), int(empty[0][1])), 5, color=(255, 255, 255), thickness=-1) opened = [] closed = [] for j in range(len(rects)): i, mask_r = rects[j] self.post('rectangle_%d' % j, mask_r) purple = cv2.bitwise_and(mask, mask_r) purple_contours = outer_contours(purple) purple_center = contour_centroid(max(purple_contours, key=contour_area)) # print(purple_center) align_angle = self.rectangles[i]['rectangle'][2] if self.rectangles[i]['rectangle'][1][1] > self.rectangles[i]['rectangle'][1][0] else self.rectangles[i]['rectangle'][2] + 90 align_angle = 360 + align_angle if align_angle < 0 else align_angle def point_in_rectangle(point, rect): contour = np.float32([cv2.boxPoints(rect)]).reshape(-1, 1, 2) return cv2.pointPolygonTest(contour, point, measureDist=False) if point_in_rectangle(purple_center, self.rectangles[i]['rectangle_org']) > 0: color = (0, 0, 255) opened.append({'center': purple_center, 'align': align_angle, 'size': self.rectangles[i]['rectangle'][1][0] * self.rectangles[i]['rectangle'][1][1], 'offset': (int(min(self.rectangles[i]['rectangle'][1]) * self.options['open_offset']), int(max(self.rectangles[i]['rectangle'][1]) * self.options['vert_offset']))}) else: color = (255, 0, 0) direction = 1 if self.rectangles[i]['rectangle'][0][0] > purple_center[0] else -1 closed.append({'center': purple_center, 'align': ((align_angle + 180) % 360) if direction == 1 else align_angle, 'size': self.rectangles[i]['rectangle'][1][0] * self.rectangles[i]['rectangle'][1][1], 'offset': (int(min(self.rectangles[i]['rectangle'][1]) * self.options['closed_offset']), int(max(self.rectangles[i]['rectangle'][1]) * self.options['vert_offset'])), 'direction': direction}) # cv2.circle(mat, purple_center, 20, color=color, thickness=-1) # draw_line(mat, *angle_to_line(self.options['manipulator_angle'], origin=purple_center), thickness=5) # draw_line(mat, *angle_to_line(align_angle, origin=purple_center), color=color, thickness=5) opened = max(opened, key=lambda x: x['size']) if opened else None closed = max(closed, key=lambda x: x['size']) if closed else None if opened: cv2.circle(mat, opened['center'], 5, color=(0, 0, 255), thickness=-1) draw_line(mat, *angle_to_line(opened['align'], origin=opened['center']), color=(0, 0, 255), thickness=5) draw_line(mat, *angle_to_line(self.options['manipulator_angle'], origin=opened['center']), thickness=5) # print('nani %d' % opened['align']) shm.recovery_vampire.open_visible.set(True) shm.recovery_vampire.open_handle_x.set(opened['center'][0]) shm.recovery_vampire.open_handle_y.set(opened['center'][1]) shm.recovery_vampire.open_offset_x.set(opened['center'][0] + opened['offset'][0]) shm.recovery_vampire.open_offset_y.set(opened['center'][1] + opened['offset'][1]) shm.recovery_vampire.open_angle_offset.set(heading_sub_degrees(self.options['manipulator_angle'], opened['align'])) shm.recovery_vampire.open_size.set(opened['size']) else: shm.recovery_vampire.open_visible.set(False) if closed: draw_line(mat, *angle_to_line(closed['align'], origin=closed['center']), color=(0, 255, 0), thickness=5) draw_line(mat, *angle_to_line(self.options['manipulator_angle'], origin=closed['center']), thickness=5) # print('what %d' % closed['align']) cv2.circle(mat, closed['center'], 5, color=(0, 255, 0), thickness=-1) shm.recovery_vampire.closed_visible.set(True) shm.recovery_vampire.closed_handle_x.set(closed['center'][0]) shm.recovery_vampire.closed_handle_y.set(closed['center'][1]) shm.recovery_vampire.closed_handle_direction.set(closed['direction']) shm.recovery_vampire.closed_offset_x.set(closed['center'][0] + closed['offset'][0]) shm.recovery_vampire.closed_offset_y.set(closed['center'][1]) shm.recovery_vampire.closed_angle_offset.set(heading_sub_degrees(self.options['manipulator_angle'], closed['align'])) shm.recovery_vampire.closed_size.set(closed['size']) else: shm.recovery_vampire.closed_visible.set(False) self.post('hmm', mat)
def desired_heading(): return shm.kalman.heading.get() - (heading_sub_degrees( call_if_function(desire), call_if_function(current), mod=mod ))
v = heading_to_vector(heading()) * -dst print(shm.path_results.center_x.get(), shm.path_results.center_y.get(), v) return abs(heading_sub_degrees(trg, heading(), math.pi*2)) < math.radians(3) and \ abs(shm.path_results.center_x.get() - v[0]) < deadband and \ abs(shm.path_results.center_y.get() - v[1]) < deadband # def on_run(self, error, p=0.0005, i=0, d=0.0, db=0.01875, max_out=0.5, negate=False, *args, **kwargs): FollowLine = Concurrent( PIDSway(shm.path_results.center_x.get, negate=True, db=0, p=1.2, i=.05), While( lambda: Sequential( FunctionTask(lambda: shm.navigation_desires.heading.set( (shm.kalman.heading.get() - .95 * math.degrees( heading_sub_degrees( math.pi / 2, shm.path_results.angle_1.get() % math.pi, math.pi * 2)) ) % 360)), #Log('{:03d} '.format(int(shm.navigation_desires.heading.get())) + s2(int(math.degrees(trg)), int(math.degrees(heading())), int(shm.desires.heading.get())) if shm.path_results.num_lines.get() == 2 else 'X'), Timer(.05)), lambda: True)) aaa = Sequential(Log("Started mission"), Depth(1.5), Log("At depth"), FakeMoveX(2, .65), Zero(), Log("Starting tracking"), Timer(1), Log("following"), Timed(Concurrent(VelocityX(.5), FollowLine), 24), Log("following done"), Timed(VelocityX(-.5), .1), Zero(), Timer(1), FakeMoveY(-.6, .65), Timed(VelocityY(.5), .05), Zero(), Timer(1), FakeMoveX(2.8, .65), Timed(VelocityX(-.5), .1), Zero(), Timer(1), Log("turning 1"), RelativeToInitialHeading(90), Log("turning 2"), Timer(1),
def PipeAlign(get_center, heading_vect, heading, get_visible, trg, dst, deadband, angle_range=math.pi * 2, abs_offset=(0, 0)): return Sequential( Log("PipeAlign start"), MasterConcurrent( Consistent(lambda: is_done(heading_vect, heading, get_center, trg, dst, deadband, abs_offset), count=.5, total=.75, invert=False, result=True), While( lambda: Sequential( #Log("attempt asdasd"), Concurrent( DownwardTarget( lambda: get_center(), target=lambda: (np.float64([heading_vect()]).view(np.complex128) * -dst).view(np.float64)[0] + abs_offset, deadband=(deadband, deadband), px=2, py=2, dx=.5, dy=.5 #, ix=.15, iy=.15 ), While( lambda: Sequential( FunctionTask( lambda: shm.navigation_desires.heading.set( (shm.kalman.heading.get( ) - .95 * math.degrees( heading_sub_degrees( trg, heading(), angle_range)) ) % 360) if get_visible() else None), #Log('{:03d} '.format(int(shm.navigation_desires.heading.get())) + s2(int(math.degrees(trg)), int(math.degrees(heading())), int(shm.desires.heading.get()))), Timer(.05)), lambda: abs( heading_sub_degrees(trg, heading(), math.pi * 2 )) > math.radians(5) ) #, count=6, total=8, invert=False, result=True), )), lambda: True #lambda: abs(heading_sub_degrees(trg, heading(), math.pi*2)) > math.radians(5) or abs(shm.path_results.center_x.get()) > .08 or abs(shm.path_results.center_y.get()) > .08 ), #, count=3, total=4, invert=False, result=True), #While(lambda: Log("V: {} h: {} d: {} x: {} y: {} c: {}".format(shm.path_results.num_lines.get(), heading(), math.degrees(heading_sub_degrees(trg, heading(), math.pi*2)), shm.path_results.center_x.get(), shm.path_results.center_y.get(), heading_to_vector(heading()) * dst)), lambda: True), #While(lambda: Log(s(int(math.degrees(heading_sub_degrees(trg, heading(), math.pi*2))) + 180)), lambda: True), #While(lambda: Log(s2(int(math.degrees(trg)), int(math.degrees(heading()))) if shm.path_results.num_lines.get() == 2 else 'X'), lambda: True), ), Log("Centered on Pipe in PipeAlign!"), #FunctionTask(lambda: shm.navigation_desires.heading.set(-180/math.pi*heading()+shm.kalman.heading.get())) )
Concurrent( DownwardTarget(lambda: (shm.path_results.center_x.get(), shm.path_results.center_y.get()), target=(lambda: heading_to_vector(heading()) * -dst), deadband=(deadband, deadband), px=1, py=1, ix=.05, iy=.05), While( lambda: Sequential( FunctionTask(lambda: shm.navigation_desires .heading.set((shm.kalman.heading.get( ) - .95 * math.degrees( heading_sub_degrees( trg, heading(), math.pi * 2))) % 360) if shm.path_results.num_lines.get( ) == 2 else None), #Log('{:03d} '.format(int(shm.navigation_desires.heading.get())) + s2(int(math.degrees(trg)), int(math.degrees(heading())), int(shm.desires.heading.get())) if shm.path_results.num_lines.get() == 2 else 'X'), Timer(.05)), lambda: abs( heading_sub_degrees(trg, heading(), math.pi * 2) ) > math.radians(5) ) #, count=6, total=8, invert=False, result=True), )), lambda: not is_done( heading, trg, dst, deadband ) #lambda: abs(heading_sub_degrees(trg, heading(), math.pi*2)) > math.radians(5) or abs(shm.path_results.center_x.get()) > .08 or abs(shm.path_results.center_y.get()) > .08 ),
global space_time global space_heading global heading_var sys.stdout.write("Press <ENTER> when sub is at %.3f heading\n" % heading) event_function() space_time = time.time() space_heading = heading_var.get() thread.start_new_thread(input_thread, ( raw_input, target_heading, )) time.sleep(0.001) initial_diff = heading_sub_degrees(target_heading, heading_var.get()) while 1: diff = heading_sub_degrees(target_heading, heading_var.get()) if initial_diff * diff < 0: sensor_time = time.time() break if space_time is None: sys.stdout.write( "The sub reached the target heading before you pressed enter!\n") while space_time is None: pass sys.stdout.write("Lag: %f\n" % (sensor_time - space_time)) sys.stdout.write("The sub was at %.3f degrees heading at enter\n" %
def is_done(heading, trg, dst, deadband): v = heading_to_vector(heading()) * -dst print(shm.path_results.center_x.get(), shm.path_results.center_y.get(), v) return abs(heading_sub_degrees(trg, heading(), math.pi*2)) < math.radians(5) and \ abs(shm.path_results.center_x.get() - v[0]) < deadband and \ abs(shm.path_results.center_y.get() - v[1]) < deadband
space_heading = None def input_thread(event_function, heading): global space_time global space_heading global heading_var sys.stdout.write("Press <ENTER> when sub is at %.3f heading\n" % heading) event_function() space_time = time.time() space_heading = heading_var.get() thread.start_new_thread(input_thread, (raw_input, target_heading,)) time.sleep(0.001) initial_diff = heading_sub_degrees(target_heading, heading_var.get()) while 1: diff = heading_sub_degrees(target_heading, heading_var.get()) if initial_diff * diff < 0: sensor_time = time.time() break if space_time is None: sys.stdout.write("The sub reached the target heading before you pressed enter!\n") while space_time is None: pass sys.stdout.write("Lag: %f\n" % (sensor_time - space_time)) sys.stdout.write("The sub was at %.3f degrees heading at enter\n" % space_heading)