def move_cursor_on_screen(self, dx=0, dy=0, dz=0): if self.rotation == 1: dx, dy = -dy, dx if self.rotation == 2: dx, dy = -dx, -dy if self.rotation == 3: dx, dy = dy, -dx v.position.x = constrain(v.position.x+dx, 0, lv.xsize-1) v.position.y = constrain(v.position.y+dy, 0, lv.ysize-1) v.position.z = constrain(v.position.z+dz, 0, lv.zsize-1) return v.position
def move_cursor_on_screen(self, dx=0, dy=0, dz=0): if self.rotation == 1: dx, dy = -dy, dx if self.rotation == 2: dx, dy = -dx, -dy if self.rotation == 3: dx, dy = dy, -dx v.position.x = constrain(v.position.x + dx, 0, lv.xsize - 1) v.position.y = constrain(v.position.y + dy, 0, lv.ysize - 1) v.position.z = constrain(v.position.z + dz, 0, lv.zsize - 1) return v.position
def lookup(self, position: Vector2): """ Look up flow field Vector value at given position :param position: Vector2 with x, y coords of considered place :return: Vector at that position (copy) """ col = utils.constrain(int(position.x / self.resolution), self.cols - 1) row = utils.constrain(int(position.y / self.resolution), self.rows - 1) return self.field[row][col]
def update(self): AMkeyPressed = False self.events = self.eventHandler.get() for event in self.events: if event.type == pygame.KEYDOWN: if event.key == pygame.K_LEFT: self.radarPosition = utils.constrain( self.radarPosition - 1, 1, 8) if event.key == pygame.K_RIGHT: self.radarPosition = utils.constrain( self.radarPosition + 1, 1, 8) if event.key == pygame.K_SPACE: self.radarOn = not self.radarOn if event.key == pygame.K_1: self.shieldOn[1] = not self.shieldOn[1] if event.key == pygame.K_2: self.shieldOn[2] = not self.shieldOn[2] if event.key == pygame.K_3: self.shieldOn[3] = not self.shieldOn[3] if event.key == pygame.K_4: self.shieldOn[4] = not self.shieldOn[4] if event.key == pygame.K_5: self.shieldOn[5] = not self.shieldOn[5] if event.key == pygame.K_6: self.shieldOn[6] = not self.shieldOn[6] if event.key == pygame.K_7: self.shieldOn[7] = not self.shieldOn[7] if event.key == pygame.K_8: self.shieldOn[8] = not self.shieldOn[8] if event.key == pygame.K_e: self.antiMissilePressed(True) AMkeyPressed = True if (event.type == pygame.KEYDOWN and event.key == pygame.K_ESCAPE) or (event.type == pygame.QUIT): self.quit = True if AMkeyPressed == False: self.antiMissilePressed(False) if self.controller: controls = self.ser.readline().strip().split(" ") self.ser.flushInput() #print(controls) self.radarPosition = int(controls[0]) if int(controls[1]): self.radarOn = not self.radarOn for i in range(0, 8): self.shieldOn[i + 1] = int(controls[i + 2]) self.antiMissilePressed(int(controls[10]))
def checkLimits(self, joint, angle_vect, angle_elem, collection): """ Check if the identify joint value is in the range imposed by the human loded into the simulation env. If not, the joint value is adjusted. Joint value is finally filtered. @param joint joint @param angle_vector vector of the joint values @param angle_elem position of the joint value to be checked in angle_vector """ if angle_vect is not None: angle = angle_vect[angle_elem] if angle is not None and joint is not None: lower,upper = joint.GetLimits() angle = utils.constrain(angle, lower, upper) collection.append(float(angle)) if len(collection) > FILTERING_SAMPLES: filtered_angles = signal.filtfilt(self.B, self.A, list(collection)) filtered_angle = filtered_angles[len(collection)-1] if filtered_angle < lower: filtered_angle = lower if filtered_angle > upper: filtered_angle = upper self.currentDOFvalues[joint.GetDOFIndex()] = filtered_angle with self.env: self.body.SetDOFValues(self.currentDOFvalues)
def start(board, steps=100, size=20): for i in range(1, steps + 1): os.system('clear') print('step:', i, '/', steps) print_board(board, size) time.sleep(0.1) board = constrain(advance(board), size)
def checkLimits(self, joint, angle_vect, angle_elem): """ Check if the identify joint value is in the range imposed by the human loded into the simulation env. If not, the joint value is adjusted @param joint joint @param angle_vector vector of the joint values @param angle_elem position of the joint value to be checked in angle_vector """ if angle_vect is not None: angle = angle_vect[angle_elem] if angle is not None and joint is not None: lower,upper = joint.GetLimits() angle = utils.constrain(angle, lower, upper) self.currentDOFvalues[joint.GetDOFIndex()] = angle with self.env: self.body.SetDOFValues(self.currentDOFvalues)
def update(self): """ Standart Euler integration Updates bahavior tree """ # updates behavior in machine state self.behavior.update(self) # Updates velocity at every step and limits it to max_speed self.velocity += self.acceleration * 1 self.velocity = limit(self.velocity, self.max_speed) # updates position self.location += self.velocity # Prevents it from crazy spinning due to very low noise speeds if self.velocity.length() > 0.5: self.rotation = atan2(self.velocity.y, self.velocity.x) # Constrains position to limits of screen self.location = constrain(self.location, SCREEN_WIDTH, SCREEN_HEIGHT) self.acceleration *= 0 # Memory of positions to draw Track self.memory_location.append((self.location.x, self.location.y)) # size of track if len(self.memory_location) > SIZE_TRACK: self.memory_location.pop(0)
def art_step(map): if map.stage is not None: stage_mode, stage_step, stage1_y, stage1_last_index = map.stage stage1_last = map.stones[stage1_last_index] if stage1_last_index is not None else None else: stage_mode = 0 stage1_y = None stage1_last = None stage_step = 0 # Color range global min_l, max_l if min_l is None: min_l = min(map.stones, key=lambda x: x.color[0]).color[0] if max_l is None: max_l = max(map.stones, key=lambda x: x.color[0]).color[0] global flower_seeds # Generate seeds if flower_seeds is None: max_x, max_y = map.size[0], map.size[1] flower_seeds = [] for i in range(9): margin_y = 180 y = map_value(i, 0, 8, margin_y, max_y - margin_y) if i % 2 == 0: x = max_x - 400.0 else: x = max_x - 1150.0 flower_seeds.append((x, y)) index, new_center, new_angle = None, None, None map.holes = [] art_center = (WORKAREA_START_X / 2, map.size[1] / 2) print stage_mode if stage_mode == 0: # Clear circle strip sel = [s for s in map.stones if not s.flag and distance(s.center, art_center) >= 100.0 * (stage_step) and distance(s.center, art_center) < 100.0 * (stage_step + 1) ] if sel: s = choice(sel) index = s.index bucket = map_value(s.color[0], min_l, max_l, 0, len(flower_seeds) + 1) bucket = constrain(int(bucket), 0, len(flower_seeds) - 1) new_center, new_angle = find_flower_pos(map, s, flower_seeds[bucket]) elif stage_mode == 1: # Fill circle strip sel = [s for s in map.stones if not s.flag and distance(s.center, art_center) >= 100.0 * (stage_step + 1) ] if sel: s = find_darkest(sel) index = s.index new_center, new_angle = find_flower_pos(map, s, art_center, max_radius=(100.0 * (stage_step + 1)), workarea=False) if new_center is None or new_angle is None: stage_mode = 0 stage_step += 1 elif stage_mode == 2: # Done pass if index is not None: force_advance = False log.debug('Art stage mode %d: stone %s => new center: %s, new angle: %s', stage_mode, index, str(new_center), str(new_angle)) else: force_advance = True stage_mode = min(stage_mode + 1, MAX_STAGE_MODE) log.debug('Art stage mode %d: None', stage_mode) stage = stage_mode, stage_step, stage1_y, stage1_last.index if stage1_last else None # Do not store in map return index, new_center, new_angle, stage, force_advance
from utils import lerp, sq, constrain from math import sqrt if __name__ == "__main__": D = 0.9 SIZE = 16 data = [] pal = [] for i in range(8): c = int(lerp(0, 255, float(i) / 7)) pal.extend([c, c, c]) im = Image.new('L', (SIZE, SIZE * 8)) im.putpalette(pal) pix = im.load() for size in range(8, 16): r = lerp(2.0, 1.0, float(size - 8) / 7) for i in range(SIZE): for j in range(SIZE): u = lerp(-r, r, float(i) / (SIZE - 1)) v = lerp(-r, r, float(j) / (SIZE - 1)) d = sq(1.4 - constrain(sqrt(sq(u) + sq(v)), 0.0, 1.4)) pix[i, j + (size - 8) * SIZE] = int(constrain(d, 0.0, 1.0) * 7) im.save("plotter-flares.png", "PNG")
#!/usr/bin/env python2 from PIL import Image from utils import constrain, lerp, dist if __name__ == "__main__": size = (128, 128) data = [] for i in range(size[0]): for j in range(size[1]): x = lerp(-1.5, 1.5, float(i) / size[0]) y = lerp(-1.5, 1.5, float(j) / size[1]) pixel = 1.0 - dist(0.0, 0.0, x, y) data.append(int(constrain(pixel, 0.0, 1.0) * 255)) light = Image.new('L', size) light.putdata(data) light.save("light.png", "PNG")
def art_step(map): if map.stage is not None: stage_mode, stage_step, stage1_y, stage1_last_index = map.stage stage1_last = map.stones[stage1_last_index] if stage1_last_index is not None else None else: stage_mode = 0 stage1_y = None stage1_last = None stage_step = 0 # Color range global min_l, max_l if min_l is None: min_l = min(map.stones, key=lambda x: x.color[0]).color[0] if max_l is None: max_l = max(map.stones, key=lambda x: x.color[0]).color[0] global flower_seeds # Generate seeds if flower_seeds is None: max_x, max_y = map.size[0], map.size[1] flower_seeds = [] for i in range(9): margin_y = 180 y = map_value(i, 0, 8, margin_y, max_y - margin_y) if i % 2 == 0: x = max_x - 400.0 else: x = max_x - 1150.0 flower_seeds.append((x, y)) index, new_center, new_angle = None, None, None # clean unusable holes map.holes = [h for h in map.holes if not in_workarea(h) and h.center[0] + h.size <= WORKAREA_START_X - (map.maxstonesize + 10) * (stage_step + 1)] if stage_mode == 0: # Clear area sel = [s for s in map.stones if not s.flag and not in_workarea(s) and s.center[0] + s.size[0] > WORKAREA_START_X - (map.maxstonesize + 10) * (stage_step + 1) and s.center[0] + s.size[0] <= WORKAREA_START_X - (map.maxstonesize + 10) * (stage_step) ] if sel: s = sel[0] index = s.index bucket = map_value(s.color[0], min_l, max_l, 0, len(flower_seeds) + 1) bucket = constrain(int(bucket), 0, len(flower_seeds) - 1) new_center, new_angle = find_flower_pos(map, s, flower_seeds[bucket]) elif stage_mode == 1: # Fill line untouched_sel = [s for s in map.stones if s.center[0] + s.size[0] <= WORKAREA_START_X - (map.maxstonesize + 10) * (stage_step + 1)] workarea_sel = [s for s in map.stones if in_workarea(s)] max_fill = 2000 rand_thresh = max(max_fill - len(workarea_sel), 0) / float(max_fill) if len(workarea_sel) > max_fill * 0.5 and random() > rand_thresh: total_sel = workarea_sel else: total_sel = workarea_sel + untouched_sel total_sel = workarea_sel + untouched_sel sel = [s for s in total_sel if not s.flag] stripes = 4 stripe_gap = map.maxstonesize stripe_width = ((1650 - 50) - stripe_gap * (stripes - 1)) / stripes if sel: if stage1_y is None: # first run of this stage stage1_y = 50 s = find_by_stripe(0, sel) stage1_last = s else: # gaps for i in range(0, stripes - 1): if stage1_y >= 50 + (stripe_width + stripe_gap) * i + stripe_width and stage1_y < 50 + (stripe_width + stripe_gap) * (i + 1): stage1_y = 50 + (stripe_width + stripe_gap) * (i + 1) break # find stripe index si = 0 for i in range(0, stripes): if stage1_y >= 50 + (stripe_width + stripe_gap) * i and stage1_y < 50 + (stripe_width + stripe_gap) * i + stripe_width: si = i break s = find_by_stripe(si, sel) stage1_y += stage1_last.size[1] + s.size[1] + 5 stage1_last = s index = s.index new_angle = 0 x = WORKAREA_START_X - (map.maxstonesize + 10) * (stage_step + 0.5) new_center = x, stage1_y if stage1_y > 1650: stage1_y = None stage_step += 1 stage_mode = 0 elif stage_mode == 2: # Done pass if index is not None: force_advance = False log.debug('Art stage mode %d: stone %s => new center: %s, new angle: %s', stage_mode, index, str(new_center), str(new_angle)) else: force_advance = True stage_mode = min(stage_mode + 1, MAX_STAGE_MODE) log.debug('Art stage mode %d: None', stage_mode) stage = stage_mode, stage_step, stage1_y, stage1_last.index if stage1_last else None # Do not store in map return index, new_center, new_angle, stage, force_advance
def art_step(map): if map.stage is not None: stage_mode, stage_step, stage1_y, stage1_last_index = map.stage stage1_last = map.stones[stage1_last_index] if stage1_last_index is not None else None else: stage_mode = 0 stage1_y = None stage1_last = None stage_step = 0 # Color range global min_l, max_l if min_l is None: min_l = min(map.stones, key=lambda x: x.color[0]).color[0] if max_l is None: max_l = max(map.stones, key=lambda x: x.color[0]).color[0] global flower_seeds # Generate seeds if flower_seeds is None: max_x, max_y = map.size[0], map.size[1] flower_seeds = [] for i in range(9): margin_y = 180 y = map_value(i, 0, 8, margin_y, max_y - margin_y) if i % 2 == 0: x = max_x - 400.0 else: x = max_x - 1150.0 flower_seeds.append((x, y)) index, new_center, new_angle = None, None, None # clean unusable holes map.holes = [ h for h in map.holes if not in_workarea(h) and h.center[0] + h.size <= WORKAREA_START_X - (map.maxstonesize + 10) * (stage_step + 1) ] art_center = (0, map.size[1] / 2) if stage_mode == 0: # Clear area sel = [ s for s in map.stones if not s.flag and not in_workarea(s) and s.center[0] + s.size[0] > WORKAREA_START_X - (map.maxstonesize + 10) * (stage_step + 1) and s.center[0] + s.size[0] <= WORKAREA_START_X - (map.maxstonesize + 10) * (stage_step) ] if sel: s = sel[0] index = s.index bucket = map_value(s.color[0], min_l, max_l, 0, len(flower_seeds) + 1) bucket = constrain(int(bucket), 0, len(flower_seeds) - 1) new_center, new_angle = find_flower_pos(map, s, flower_seeds[bucket]) elif stage_mode == 1: # Fill line untouched_sel = [ s for s in map.stones if s.center[0] + s.size[0] <= WORKAREA_START_X - (map.maxstonesize + 10) * (stage_step + 1) ] workarea_sel = [s for s in map.stones if in_workarea(s)] max_fill = 2000 rand_thresh = max(max_fill - len(workarea_sel), 0) / float(max_fill) if len(workarea_sel) > max_fill * 0.5 and random() > rand_thresh: total_sel = workarea_sel else: total_sel = workarea_sel + untouched_sel total_sel = workarea_sel + untouched_sel sel = [s for s in total_sel if not s.flag] if sel: if stage1_y is None: # first run of this stage stage1_y = 50 # pick random stone if stage1_last is not None: s = find_most_distant_color(stage1_last, sel) else: s = choice(sel) stage1_last = s else: s = find_best_match(stage1_last, sel) rc = abs(math.cos(math.radians(stage1_last.angle))) rs = abs(math.sin(math.radians(stage1_last.angle))) a, b = stage1_last.size[1] * rc, stage1_last.size[0] * rs c, d = s.size[1] * rc, s.size[0] * rs e, f = math.sqrt(a * a + b * b), math.sqrt(c * c + d * d) stage1_y += e + f + 5 stage1_last = s index = s.index x = WORKAREA_START_X - (map.maxstonesize + 10) * (stage_step + 0.5) new_center = x, stage1_y new_angle = atan_angle(art_center, new_center) if stage1_y > 1650: stage1_y = None stage_step += 1 stage_mode = 0 elif stage_mode == 2: # Done pass if index is not None: force_advance = False log.debug( "Art stage mode %d: stone %s => new center: %s, new angle: %s", stage_mode, index, str(new_center), str(new_angle), ) else: force_advance = True stage_mode = min(stage_mode + 1, MAX_STAGE_MODE) log.debug("Art stage mode %d: None", stage_mode) stage = stage_mode, stage_step, stage1_y, stage1_last.index if stage1_last else None # Do not store in map return index, new_center, new_angle, stage, force_advance
if __name__ == "__main__": image = Image.open(sys.argv[1]) image = image.convert("L") bumpmap = array("H") name = os.path.splitext(sys.argv[1])[0] + ".bin" for y in range(image.size[1]): for x in range(image.size[0]): this = image.getpixel((x, y)) if y < image.size[1] - 1: down = image.getpixel((x, y + 1)) else: down = this if x < image.size[0] - 1: right = image.getpixel((x + 1, y)) else: right = this # scale down the difference between pixels du = (this - down) * 0.25 dv = (this - right) * 0.25 # light texture size is 128 * 128 u = (constrain(int(du), -64, 63) + y) & 127 v = (constrain(int(dv), -64, 63) + x) & 127 bumpmap.append(u * 128 + v) bumpmap.byteswap() with open(name, "w") as f: bumpmap.tofile(f)
for i in range(8): pal.extend([0, 0, 0]) for i in range(16): c = int(lerp(0, 255, float(i + 1) / 16)) pal.extend([c, c, c]) for i in range(4): pal.extend([255, 255, 255]) for i in range(4): c = int(lerp(255, 0, float(i + 1) / 4)) pal.extend([c, c, c]) for i in range(size[0]): for j in range(size[1]): x = lerp(-D, D, float(i) / size[0]) y = lerp(-D, D, float(j) / size[1]) d = dist(x, y, 0, 0) if d < D: p = constrain(int(sq(1.0 - d) * 128), 0, 31) else: p = 0 data.append(int(p)) im = Image.new('L', size) im.putdata(data) im.putpalette(pal) im.save(sys.argv[1], "PNG")
def art_step(map): if map.stage is not None: stage_mode, stage_step, stage1_y, stage1_last_index = map.stage stage1_last = map.stones[ stage1_last_index] if stage1_last_index is not None else None else: stage_mode = 0 stage1_y = None stage1_last = None stage_step = 0 # Color range global min_l, max_l if min_l is None: min_l = min(map.stones, key=lambda x: x.color[0]).color[0] if max_l is None: max_l = max(map.stones, key=lambda x: x.color[0]).color[0] global flower_seeds # Generate seeds if flower_seeds is None: max_x, max_y = map.size[0], map.size[1] flower_seeds = [] for i in range(9): margin_y = 180 y = map_value(i, 0, 8, margin_y, max_y - margin_y) if i % 2 == 0: x = max_x - 400.0 else: x = max_x - 1150.0 flower_seeds.append((x, y)) index, new_center, new_angle = None, None, None # clean unusable holes map.holes = [ h for h in map.holes if not in_workarea(h) and h.center[0] + h.size <= WORKAREA_START_X - (map.maxstonesize + 10) * (stage_step + 1) ] if stage_mode == 0: # Clear area sel = [ s for s in map.stones if not s.flag and not in_workarea(s) and s.center[0] + s.size[0] > WORKAREA_START_X - (map.maxstonesize + 10) * (stage_step + 1) and s.center[0] + s.size[0] <= WORKAREA_START_X - (map.maxstonesize + 10) * (stage_step) ] if sel: s = sel[0] index = s.index bucket = map_value(s.color[0], min_l, max_l, 0, len(flower_seeds) + 1) bucket = constrain(int(bucket), 0, len(flower_seeds) - 1) new_center, new_angle = find_flower_pos(map, s, flower_seeds[bucket]) elif stage_mode == 1: # Fill line untouched_sel = [ s for s in map.stones if s.center[0] + s.size[0] <= WORKAREA_START_X - (map.maxstonesize + 10) * (stage_step + 1) ] workarea_sel = [s for s in map.stones if in_workarea(s)] max_fill = 2000 rand_thresh = max(max_fill - len(workarea_sel), 0) / float(max_fill) if len(workarea_sel) > max_fill * 0.5 and random() > rand_thresh: total_sel = workarea_sel else: total_sel = workarea_sel + untouched_sel total_sel = workarea_sel + untouched_sel sel = [s for s in total_sel if not s.flag] stripes = 4 stripe_gap = map.maxstonesize stripe_width = ((1650 - 50) - stripe_gap * (stripes - 1)) / stripes if sel: if stage1_y is None: # first run of this stage stage1_y = 50 s = find_by_stripe(0, sel) stage1_last = s else: # gaps for i in range(0, stripes - 1): if stage1_y >= 50 + ( stripe_width + stripe_gap ) * i + stripe_width and stage1_y < 50 + ( stripe_width + stripe_gap) * (i + 1): stage1_y = 50 + (stripe_width + stripe_gap) * (i + 1) break # find stripe index si = 0 for i in range(0, stripes): if stage1_y >= 50 + (stripe_width + stripe_gap) * i and stage1_y < 50 + ( stripe_width + stripe_gap) * i + stripe_width: si = i break s = find_by_stripe(si, sel) stage1_y += stage1_last.size[1] + s.size[1] + 5 stage1_last = s index = s.index new_angle = 0 x = WORKAREA_START_X - (map.maxstonesize + 10) * (stage_step + 0.5) new_center = x, stage1_y if stage1_y > 1650: stage1_y = None stage_step += 1 stage_mode = 0 elif stage_mode == 2: # Done pass if index is not None: force_advance = False log.debug( 'Art stage mode %d: stone %s => new center: %s, new angle: %s', stage_mode, index, str(new_center), str(new_angle)) else: force_advance = True stage_mode = min(stage_mode + 1, MAX_STAGE_MODE) log.debug('Art stage mode %d: None', stage_mode) stage = stage_mode, stage_step, stage1_y, stage1_last.index if stage1_last else None # Do not store in map return index, new_center, new_angle, stage, force_advance
def cruise(): """ Tracks the black line. Acquires images from front camera and uses it to do pure pursuit. Uses functions in driver.py to drive the pi car. There is a three-step process to reach the goal. Step 1. Employs CameraCapturer class to acquire images from front camera and rectify lens distortion. Step 2. Chooses the ROI and binarizes the it. Then uses morphology method to get the target point. Step 3. According to target point, applies pure pursuit algorithm and uses functions in driver.py to drive the car. Args: None Returns: None """ # Initialize CameraCapturer and drive cap = camera_capturer.CameraCapturer("front") d = driver.driver() d.setStatus(motor=0.4, servo=0, mode="speed") last_time = time.time() target = OFFSET # -int(cap.width / 5) # Parameters of PID controller kp = 1.2 ki = 0 kd = 0.1 # Initialize error to 0 for PID controller error_p = 0 error_i = 0 error_d = 0 error = 0 servo = 0 last_servo = 0 last_angle = 0 n = 0.2 try: while True: this_time = time.time() if this_time - last_time > PERIOD: # Execute the code below every # PERIOD time last_time = this_time # d.setStatus(motor=0, servo=n, mode="speed") # n = -n # continue # ----------------------------------------------------------- # # Start your code here # # Image processing. Outputs a target_point. frame = cap.get_frame() start = time.time() skel, img_bin_rev = image_processing.image_process(frame) white_rate = \ np.size(img_bin_rev[img_bin_rev == 255]) / img_bin_rev.size if white_rate > 0.3: print("stay", white_rate) d.setStatus(servo=last_servo) continue target_point, width, _, img_DEBUG, angle = \ choose_target_point(skel, target) end = time.time() if angle == 0: angle = last_angle pass else: # Update the PID error error_p = angle # **(9/7) error_i += error_p error_d = error_p - error error = error_p # PID controller servo = utils.constrain( -kp * error_p - ki * error_i - kd * error_d, 1, -1) d.setStatus(servo=servo) last_servo = servo # print("servo: ", servo, "error_p: ", error_p) # img_DEBUG[:, target] = 255 # if DEBUG: # # cv.imshow("frame", frame) # cv.imshow("img_bin_rev", img_bin_rev) # cv.imshow("img_DEBUG", img_DEBUG) # cv.waitKey(300) # ----------------------------------------------------------- # else: # time.sleep(0.01) pass except KeyboardInterrupt: d.setStatus(servo=0, motor=0)
def parking(): """ Tracks the black line. Acquires images from front camera and uses it to do pure pursuit. Uses functions in driver.py to drive the pi car. There is a three-step process to reach the goal. Step 1. Employs CameraCapturer class to acquire images from front camera and rectify lens distortion. Step 2. Chooses the ROI and binarizes the it. Then uses morphology method to get the target point. Step 3. According to target point, applies pure pursuit algorithm and uses functions in driver.py to drive the car. Args: None Returns: None """ # Initialize CameraCapturer and drive # cap = camera_capturer.CameraCapturer("rear") # d = driver.driver() # d.setStatus(motor=0.2, servo=0, mode="speed") # parking_state # 0: initial state # 1: on parking # 2: waiting for another parking instruction parking_state = 0 target_lot = 3 current_lot = 3 # cap = camera_capturer.CameraCapturer("rear") # d = driver.driver() # d.setStatus(motor=0.4, servo=0, mode="speed") target_x = 0 target_y = 0 while True: if parking_state == 0: print("------------------------------------------------------") print("Which lot would you like to park? :-)") try: target_lot = int(input()) if target_lot == -1: break elif target_lot < 0 or target_lot > 4: raise lotOutOfRangeError( "The lot number must be 1, 2, 3 or 4.") except: print("Invalid input! The lot number must be 1, 2, 3 or 4.") continue parking_state = 1 if parking_state == 1: print("Start parking...") print("Target lot: %d" % (target_lot)) # Start # img = cap.get_frame() img = get_frame() print("New image catched!") width = img.shape[1] height = img.shape[0] roi = img[int(height * 2 / 5):height, 0:int(width // 2), :] lines, linePoints = line_detection.line_detection(roi) result = line_detection.digit_detection(roi, lines, linePoints) result = result[1:] print("result", result) if result[0] in [1, 2, 3, 4]: current_lot = result[0] delta = current_lot - target_lot if delta == 0: pass else: parking_control.gostraight(delta * LOT_LENGTH) img = get_frame() print("New image catched!") width = img.shape[1] height = img.shape[0] roi = img[int(height * 2 / 5):height, 0:int(width // 2), :] lines, linePoints = line_detection.line_detection(roi) result = line_detection.digit_detection( roi, lines, linePoints) result = result[1:] print("result", result) else: current_lot = 2 delta = current_lot - target_lot if delta == 0: pass else: parking_control.gostraight(delta * LOT_LENGTH) img = get_frame() print("New image catched!") width = img.shape[1] height = img.shape[0] roi = img[int(height * 2 / 5):height, 0:int(width // 2), :] lines, linePoints = line_detection.line_detection(roi) result = line_detection.digit_detection( roi, lines, linePoints) result = result[1:] print("result", result) x, y = coordinate_transform(result) print("x", x, "y", y) x = utils.constrain(x, 40, 20) y = utils.constrain(y, 82, 70) print("x", x, "y", y) target_x = x + LOT_WIDTH // 2 + 5 target_y = y - LOT_LENGTH // 2 + 5 print("target", target_x, target_y) parking_control.park(target_x, target_y, 0) parking_control.gostraight(-5) current_lot = target_lot print("Parking finished!") parking_state = 2 if parking_state == 2: print("------------------------------------------------------") print("Which lot would you like to park? :-)") try: target_lot = int(input()) if target_lot == -1: break elif target_lot < 0 or target_lot > 4: raise lotOutOfRangeError( "The lot number must be 1, 2, 3 or 4.") except: print("Invalid input! The lot number must be 1, 2, 3 or 4.") continue if current_lot == target_lot: print("Already in lot %d" % (current_lot)) else: parking_control.gostraight(5) parking_control.reverse(target_x, target_y) # delta = current_lot - target_lot # print("distance", delta*LOT_LENGTH) # parking_control.gostraight(delta*LOT_LENGTH) parking_state = 1
def art_step(map): if map.stage is not None: stage_mode, stage_step, stage1_y, stage1_last_index = map.stage stage1_last = map.stones[ stage1_last_index] if stage1_last_index is not None else None else: stage_mode = 0 stage1_y = None stage1_last = None stage_step = 0 # Color range global min_l, max_l if min_l is None: min_l = min(map.stones, key=lambda x: x.color[0]).color[0] if max_l is None: max_l = max(map.stones, key=lambda x: x.color[0]).color[0] global flower_seeds # Generate seeds if flower_seeds is None: max_x, max_y = map.size[0], map.size[1] flower_seeds = [] for i in range(9): margin_y = 180 y = map_value(i, 0, 8, margin_y, max_y - margin_y) if i % 2 == 0: x = max_x - 400.0 else: x = max_x - 1150.0 flower_seeds.append((x, y)) index, new_center, new_angle = None, None, None # clean unusable holes map.holes = [ h for h in map.holes if not in_workarea(h) and h.center[0] + h.size <= WORKAREA_START_X - (map.maxstonesize + 10) * (stage_step + 1) ] art_center = (0, map.size[1] / 2) if stage_mode == 0: # Clear area sel = [ s for s in map.stones if not s.flag and not in_workarea(s) and s.center[0] + s.size[0] > WORKAREA_START_X - (map.maxstonesize + 10) * (stage_step + 1) and s.center[0] + s.size[0] <= WORKAREA_START_X - (map.maxstonesize + 10) * (stage_step) ] if sel: s = sel[0] index = s.index bucket = map_value(s.color[0], min_l, max_l, 0, len(flower_seeds) + 1) bucket = constrain(int(bucket), 0, len(flower_seeds) - 1) new_center, new_angle = find_flower_pos(map, s, flower_seeds[bucket]) elif stage_mode == 1: # Fill line untouched_sel = [ s for s in map.stones if s.center[0] + s.size[0] <= WORKAREA_START_X - (map.maxstonesize + 10) * (stage_step + 1) ] workarea_sel = [s for s in map.stones if in_workarea(s)] max_fill = 2000 rand_thresh = max(max_fill - len(workarea_sel), 0) / float(max_fill) if len(workarea_sel) > max_fill * 0.5 and random() > rand_thresh: total_sel = workarea_sel else: total_sel = workarea_sel + untouched_sel total_sel = workarea_sel + untouched_sel sel = [s for s in total_sel if not s.flag] if sel: if stage1_y is None: # first run of this stage stage1_y = 50 # pick random stone if stage1_last is not None: s = find_most_distant_color(stage1_last, sel) else: s = choice(sel) stage1_last = s else: s = find_best_match(stage1_last, sel) rc = abs(math.cos(math.radians(stage1_last.angle))) rs = abs(math.sin(math.radians(stage1_last.angle))) a, b = stage1_last.size[1] * rc, stage1_last.size[0] * rs c, d = s.size[1] * rc, s.size[0] * rs e, f = math.sqrt(a * a + b * b), math.sqrt(c * c + d * d) stage1_y += e + f + 5 stage1_last = s index = s.index x = WORKAREA_START_X - (map.maxstonesize + 10) * (stage_step + 0.5) new_center = x, stage1_y new_angle = atan_angle(art_center, new_center) if stage1_y > 1650: stage1_y = None stage_step += 1 stage_mode = 0 elif stage_mode == 2: # Done pass if index is not None: force_advance = False log.debug( 'Art stage mode %d: stone %s => new center: %s, new angle: %s', stage_mode, index, str(new_center), str(new_angle)) else: force_advance = True stage_mode = min(stage_mode + 1, MAX_STAGE_MODE) log.debug('Art stage mode %d: None', stage_mode) stage = stage_mode, stage_step, stage1_y, stage1_last.index if stage1_last else None # Do not store in map return index, new_center, new_angle, stage, force_advance
for i in range(8): pal.extend([0, 0, 0]) for i in range(16): c = int(lerp(0, 255, float(i + 1) / 16)) pal.extend([c, c, c]) for i in range(4): pal.extend([255, 255, 255]) for i in range(4): c = int(lerp(255, 0, float(i + 1) / 4)) pal.extend([c, c, c]) for i in range(size[0]): for j in range(size[1]): x = lerp(-D, D, float(i) / size[0]) y = lerp(-D, D, float(j) / size[1]) d = dist(x, y, 0, 0); if d < D: p = constrain(int(sq(1.0 - d) * 128), 0, 31) else: p = 0 data.append(int(p)) im = Image.new('L', size) im.putdata(data) im.putpalette(pal) im.save("metaball.png", "PNG")
#!/usr/bin/env python3 from utils import constrain, lerp, dist import sys if __name__ == '__main__': size = (128, 128) print('const u_short light_w = 128;') print('const u_short light_h = 128;') print('') print('u_char %s[] = {' % sys.argv[1]) for i in range(size[0]): sys.stdout.write(' ') for j in range(size[1]): x = lerp(-1.5, 1.5, float(i) / size[0]) y = lerp(-1.5, 1.5, float(j) / size[1]) pixel = 1.0 - dist(0.0, 0.0, x, y) sys.stdout.write(' 0x%02x,' % int(constrain(pixel, 0.0, 1.0) * 255)) sys.stdout.write('\n') print('};')