def get_combat_zones(gamestate): 'get all my fighter ants in groups' group_distance = 3**2 enemy_distance = gamestate.euclidean_distance_add(gamestate.attackradius2, 3) enemy_ants = [ant_loc for ant_loc, owner in gamestate.enemy_ants()] open_fighters = path.bfs(gamestate, enemy_ants, enemy_distance, lambda loc : gamestate.is_my_unmoved_ant(loc)) open_supporters = path.bfs(gamestate, open_fighters, group_distance, lambda loc : gamestate.is_my_unmoved_ant(loc) and loc not in open_fighters) all_my_ants = [ant for ant in gamestate.my_ants() if ant not in open_fighters] # set open fighters to gamestate gamestate.my_fighters = open_fighters fighter_groups = [] while len(open_fighters) > 0: first_ant = open_fighters.pop() group = [first_ant] group_i = 0 while len(group) > group_i: fighter_friends = [ant for ant in open_fighters if gamestate.euclidean_distance2(group[group_i], ant) < group_distance] supporter_friends = [ant for ant in open_supporters if gamestate.euclidean_distance2(group[group_i], ant) < group_distance] group.extend(fighter_friends) group.extend(supporter_friends) open_fighters = [ant for ant in open_fighters if ant not in fighter_friends] open_supporters = [ant for ant in open_supporters if ant not in supporter_friends] group_i += 1 group_enemy = path.bfs(gamestate, group, enemy_distance, lambda loc : loc in enemy_ants) fighter_groups.append((group, group_enemy)) return fighter_groups
def shortest_path_backward_bfs(self, first_vertex, second_vertex): b = [] a = bfs(self, first_vertex) b = getPath(a, second_vertex) return b[0], b[1]
def set_value(self, loc, range, value): 'set value on given loc, with its influence reaching up to range' # a simple/different model for diffusion is used self.map[loc] = value all_neighbours = path.bfs(self.gamestate, [loc], range**2, lambda x : True) for n_loc in all_neighbours: self.map[n_loc] = value / self.gamestate.manhattan_distance(loc, n_loc)
def slam(ir_values, laser_values, mode, steering_cmd_man, pid_cons, gyro_value, num_laser_values, num_hall_reads, grid, shared_pos, move_cmd, has_new_ir): """ Main loop for driving the robot. """ global pid, state, robot_pos, robot_dir, travel_commands, verified_grids, wheel_distance, driven_path, tile_wall_count, update_ir_map, first_laser_read, ir_grid, ir_enabled # Open and clear the uart port to the steering module st.open_port() st.steering_port.flushInput() st.set_speed(AUTO_MODE, 0) st.forward() # PID vars pid_side = RIGHT control.forward_sensor = IR_ID[FORWARD_RIGHT] control.backward_sensor = IR_ID[BACKWARD_RIGHT] full_turn_start = time.time() # Laser vars adjusted_laser = [] last_laser_list = [] # Position vars start_pos = [pos for pos in robot_pos] driven_path = [] prev_state = MANUAL # SLAM vars laser_counter = 0 mapper = sm.Mapping() # Other search_mode = LASER debug_print = True laser_floor = [[0 for x in range(31)] for y in range(31)] while True: get_distance() update_path(robot_pos, shared_pos) #Updates final map with ir values update_final_ir(grid, ir_grid) if not mode.value: state = MANUAL if state != prev_state and debug_print: print('---------------') print('CURRENT STATE: ' + STATES[state]) print('CURRENT DIRECTION: ' + DIRECTIONS[robot_dir]) print('CURRENT POSITION: ' + str(robot_pos)) print('PID SIDE: ' + str(pid_side)) # Update pid values from the laptop program with pid_cons.get_lock(): if pid_cons[0] != -1: if pid_cons[0] == 0: pid.Kp = pid_cons[1] elif pid_cons[0] == 1: pid.Ki = pid_cons[1] elif pid_cons[0] == 2: pid.Kd = pid_cons[1] pid_cons[0] = -1 print("PID: " + str(pid.Kp) + " " + str(pid.Ki) + " " + str(pid.Kd)) # Initial state. Should not enter here again if state == START: prev_state = START # Assign values to surrounding tiles from START position set_grid_value_2d(15, 14, EMPTY_TILE, ir_grid) set_grid_value_2d(15, 15, EMPTY_TILE, ir_grid) set_grid_value_2d(15, 16, WALL_TILE, ir_grid) set_grid_value_2d(14, 15, WALL_TILE, ir_grid) set_grid_value_2d(16, 15, WALL_TILE, ir_grid) # Enter WALL to exit the starting cell state = WALL continue # Main state when following walls elif state == WALL: prev_state = WALL # If the room is closed, go home if path.closed_room(grid, robot_pos): state = TRAVEL ir_enabled = False continue # Update map if has_new_ir.value == 1 and check_centered() and ir_enabled: update_ir_map = True map_ir_neighbors(ir_grid, robot_pos, robot_dir, ir_values, tile_wall_count, has_new_ir) # Enters SLAM state if there arent any walls around if search_mode == LASER and not [ir_values[i] for i in range(NUM_SENSORS) if ir_values[i] != EMPTY_CHECK and i != 1]: state = SLAM # Break and start the laser tower st.set_speed(AUTO_MODE, 0, 'both') continue # Check for a wall in front of the robot if ir_values[IR_ID[FORWARD]] <= FORWARD_CHECK: #set front as wall _x, _y = GRID_INDEX_VALUES[robot_dir][0] set_grid_value_2d(int(robot_pos[0])+_x, int(robot_pos[1])+_y, WALL_TILE, ir_grid) tile_wall_count[(int(robot_pos[0]) + _x, int(robot_pos[1]) + _y)] = [100,100,0] # Switch to dead end state if there are walls on both sides of the robot if ir_values[IR_ID[FORWARD_RIGHT]] < EMPTY_CHECK and ir_values[IR_ID[FORWARD_LEFT]] < EMPTY_CHECK: state = DEAD_END # Switch to turn left state if there is a wall on the right elif ir_values[IR_ID[FORWARD_RIGHT]] < EMPTY_CHECK: state = TURN_LEFT # Switch to turn right state if there is a wall on the left elif ir_values[IR_ID[FORWARD_LEFT]] < EMPTY_CHECK: state = TURN_RIGHT # Find closest unexplored if there are no walls on the sides else: #map_ir_neighbors(ir_grid, robot_pos, robot_dir, ir_values, tile_wall_count) _x, _y = GRID_INDEX_VALUES[robot_dir][2] set_grid_value_2d(int(robot_pos[0])+_x, int(robot_pos[1])+_y, EMPTY_TILE, ir_grid) tile_wall_count[(int(robot_pos[0]) + _x, int(robot_pos[1]) + _y)] = [100,0,100] _x, _y = GRID_INDEX_VALUES[robot_dir][3] set_grid_value_2d(int(robot_pos[0])+_x, int(robot_pos[1])+_y, EMPTY_TILE, ir_grid) tile_wall_count[(int(robot_pos[0]) + _x, int(robot_pos[1]) + _y)] = [100,0,100] update_ir_map = True state = TRAVEL continue # Check if the wall ends on the left side if ir_values[IR_ID[FORWARD_LEFT]] == EMPTY_CHECK and ir_values[IR_ID[BACKWARD_LEFT]] < EMPTY_CHECK: num_closed_walls = 0 # Number of closed walls in turn st.set_speed(AUTO_MODE, st.speeds[st.DEFAULT]) # Drive out past walls and stop while ir_values[IR_ID[BACKWARD_LEFT]] != EMPTY_CHECK: get_distance() # Check forward distance if ir_values[IR_ID[FORWARD]] < EMPTY_CHECK and ir_values[IR_ID[FORWARD]] > 12: _x, _y = GRID_INDEX_VALUES[robot_dir][0] set_grid_value_2d(int(robot_pos[0])+_x, int(robot_pos[1])+_y, WALL_TILE, ir_grid) tile_wall_count[(int(robot_pos[0]) + _x, int(robot_pos[1]) + _y)] = [100,100,0] print('FRONT WALL DETECTED') num_closed_walls += 1 while ir_values[IR_ID[FORWARD]] > 12: pass else: go_tiles_simple(0.3) st.set_speed(AUTO_MODE, 0) if ir_values[IR_ID[FORWARD_RIGHT]] < EMPTY_CHECK and ir_values[IR_ID[BACKWARD_RIGHT]] < EMPTY_CHECK: print('RIGHT WALL DETECTED') num_closed_walls += 1 st.set_speed(AUTO_MODE, 0) # Update the map #for i in range(100): print('Waiting for new ir') if ir_enabled: while has_new_ir.value == 0: pass map_ir_neighbors(ir_grid, robot_pos, robot_dir, ir_values, tile_wall_count, has_new_ir) update_ir_map = True # Stay here if the robot is using the laser if search_mode == LASER and num_closed_walls < 2: print('LASER SEARCH ACTIVE, SKIPPING REST AND ENTER SLAM') state = SLAM continue # Wall continues on the right side: Follow it else: print('WALL CONTINUES ON THE RIGHT SIDE, FOLLOW IT') pid_side = RIGHT continue """ # Wall continues on the right side: Follow it if ir_values[IR_ID[FORWARD_RIGHT]] <= RIGHT_CHECK and ir_values[IR_ID[BACKWARD_RIGHT]] <= RIGHT_CHECK: print('WALL CONTINUES ON THE RIGHT SIDE, FOLLOW IT') pid_side = RIGHT continue # The robot is either standing in a t-turn or a four-turn else: print('T OR 4 TURN') state = TRAVEL continue """ continue # Check if the wall ends on the right side if ir_values[IR_ID[FORWARD_RIGHT]] == EMPTY_CHECK and ir_values[IR_ID[BACKWARD_RIGHT]] < EMPTY_CHECK: num_closed_walls = 0 # Number of closed walls in turn st.set_speed(AUTO_MODE, st.speeds[st.DEFAULT]) # Drive out past walls and stop while ir_values[IR_ID[BACKWARD_RIGHT]] != EMPTY_CHECK: get_distance() # Check forward distance if ir_values[IR_ID[FORWARD]] < EMPTY_CHECK and ir_values[IR_ID[FORWARD]] > 12: _x, _y = GRID_INDEX_VALUES[robot_dir][0] set_grid_value_2d(int(robot_pos[0])+_x, int(robot_pos[1])+_y, WALL_TILE, ir_grid) tile_wall_count[(int(robot_pos[0]) + _x, int(robot_pos[1]) + _y)] = [100,100,0] num_closed_walls += 1 while ir_values[IR_ID[FORWARD]] > 12: pass else: go_tiles_simple(0.3) st.set_speed(AUTO_MODE, 0) if ir_values[IR_ID[FORWARD_LEFT]] < EMPTY_CHECK and ir_values[IR_ID[BACKWARD_LEFT]] < EMPTY_CHECK: print('LEFT WALL DETECTED') num_closed_walls += 1 # Update the map if ir_enabled: while has_new_ir.value == 0: pass map_ir_neighbors(ir_grid, robot_pos, robot_dir, ir_values, tile_wall_count, has_new_ir) update_ir_map = True # Stay here if the robot is using the laser if search_mode == LASER and num_closed_walls < 2: print('LASER SEARCH ACTIVE, SKIPPING REST AND ENTER SLAM') state = SLAM continue # Wall continues on the left side: Follow it else: print('WALL CONTINUES ON THE LEFT SIDE, FOLLOW IT') pid_side = LEFT continue """ if ir_values[IR_ID[FORWARD_LEFT]] <= LEFT_CHECK and ir_values[IR_ID[BACKWARD_LEFT]] <= LEFT_CHECK: print('WALL CONTINUES ON THE LEFT SIDE, FOLLOW IT') pid_side = LEFT continue # The robot is either standing in a t-turn or a four-turn else: print('T OR 4 TURN') state = TRAVEL continue """ continue # Use pid to follow the wall pid_side = detect_walls(ir_values) if pid_side == LEFT: pid.reversed = True elif pid_side == RIGHT: pid.reversed = False if pid_side != None: pid.enabled = True turn_factor = pid.compute(ir_values) if st.speeds[st.LEFT] != abs(int(st.speeds[st.DEFAULT] - turn_factor)): st.set_speed(AUTO_MODE, abs(st.speeds[st.DEFAULT] - turn_factor), "left") if st.speeds[st.RIGHT] != abs(int(st.speeds[st.DEFAULT] + turn_factor)): st.set_speed(AUTO_MODE, abs(st.speeds[st.DEFAULT] + turn_factor), "right") else: pid.enabled = False if st.speeds[st.LEFT] != abs(int(st.speeds[st.DEFAULT])): st.set_speed(AUTO_MODE, abs(st.speeds[st.DEFAULT]), "left") if st.speeds[st.RIGHT] != abs(int(st.speeds[st.DEFAULT])): st.set_speed(AUTO_MODE, abs(st.speeds[st.DEFAULT]), "right") continue elif state == TRAVEL: prev_state = TRAVEL # Generate robot commands which, when executed, # will drive the robot to the nearest unexplored tile if not travel_commands: if path.closed_room(grid, robot_pos): p = path.a_star(grid, robot_pos, [15, 15]) ir_enabled = False else: p = path.find_closest_unexplored(grid, robot_pos) travel_commands = path.follow_path(p, robot_dir) print('GENERATED COMMANDS: ' + str(travel_commands)) # Excute travel commands if travel_commands: c = travel_commands.pop(0) move_cmd.value = c parts = c.split('_') if parts[0] == "go": tiles = int(parts[2]) go_tiles(tiles, ir_values, shared_pos, ir_grid) elif parts[0] == "turn": turn_deg = int(parts[2]) turn(turn_deg, parts[1], ir_values) time.sleep(0.2) # Check if home print('CHECKING IF HOME', str(robot_pos)) if not travel_commands and int_robot_pos(robot_pos) == int_robot_pos(start_pos): st.set_speed(AUTO_MODE, 0) mode.value = 0 state = MANUAL continue elif not travel_commands: if ir_values[IR_ID[FORWARD_LEFT]] < EMPTY_CHECK and ir_values[IR_ID[FORWARD_RIGHT]] < EMPTY_CHECK: state = WALL elif (ir_values[IR_ID[FORWARD_LEFT]] < EMPTY_CHECK and ir_values[IR_ID[FORWARD]] < EMPTY_CHECK) or \ (ir_values[IR_ID[FORWARD_RIGHT]] < EMPTY_CHECK and ir_values[IR_ID[FORWARD]] < EMPTY_CHECK): for i in range(4): while has_new_ir.value == 0: pass current_ir_values = [ir for ir in ir_values] map_ir_neighbors(ir_grid, robot_pos, robot_dir, current_ir_values, tile_wall_count, has_new_ir) update_ir_map = True update_final_ir(grid, ir_grid) state = TRAVEL else: state = SLAM continue else: state = WALL continue elif state == TURN_LEFT: prev_state = TURN_LEFT turn(90, 'left', ir_values) # Reset verify counter on neighbour tiles so they are verified again after the turn reset_neighbors(robot_pos, tile_wall_count) # Force a new position after a turn to keep the robot aligned with the grid int_x = int(robot_pos[0]) int_y = int(robot_pos[1]) robot_pos[0] = int_x + 0.5 robot_pos[1] = int_y + 0.5 state = WALL continue elif state == TURN_RIGHT: prev_state = TURN_RIGHT turn(90, 'right', ir_values) # Reset verify counter on neighbour tiles so they are verified again after the turn reset_neighbors(robot_pos, tile_wall_count) # Force a new position after a turn to keep the robot aligned with the grid int_x = int(robot_pos[0]) int_y = int(robot_pos[1]) robot_pos[0] = int_x + 0.5 robot_pos[1] = int_y + 0.5 state = WALL continue elif state == DEAD_END: prev_state = DEAD_END #front _x, _y = GRID_INDEX_VALUES[robot_dir][0] set_grid_value_2d(int(robot_pos[0])+_x, int(robot_pos[1])+_y, WALL_TILE, ir_grid) tile_wall_count[(int(robot_pos[0]) + _x, int(robot_pos[1]) + _y)] = [100,100,0] #right _x, _y = GRID_INDEX_VALUES[robot_dir][2] set_grid_value_2d(int(robot_pos[0])+_x, int(robot_pos[1])+_y, WALL_TILE, ir_grid) tile_wall_count[(int(robot_pos[0]) + _x, int(robot_pos[1]) + _y)] = [100,100,0] #left _x, _y = GRID_INDEX_VALUES[robot_dir][3] set_grid_value_2d(int(robot_pos[0])+_x, int(robot_pos[1])+_y, WALL_TILE, ir_grid) tile_wall_count[(int(robot_pos[0]) + _x, int(robot_pos[1]) + _y)] = [100,100,0] turn(180, 'left', ir_values) state = WALL continue elif state == SLAM: prev_state = SLAM pid.enabled = False if not laser_counter: for i in range(4): while has_new_ir.value == 0: pass current_ir_values = [ir for ir in ir_values] map_ir_neighbors(ir_grid, robot_pos, robot_dir, current_ir_values, tile_wall_count, has_new_ir) update_ir_map = True update_final_ir(grid, ir_grid) if not first_laser_read: for y in range(MAP_SIZE): for x in range(MAP_SIZE): set_grid_value(x,y, ir_grid[y][x], grid) #print('\nIN SLAM STATE, SET SPEED 0') st.set_speed(AUTO_MODE, 0) time.sleep(0.5) if path.closed_room(grid, robot_pos): # Find closest path home print('ROOM IS CLOSED, GO HOME') stop_tower(num_hall_reads) path_home = path.a_star(grid, robot_pos, [15.5, 15.5]) travel_commands = path.follow_path(path_home, robot_dir) print("Path home: " + str(travel_commands)) state = TRAVEL ir_enabled = False continue else: # Check if closest unexplored tile is too far away, if so then go there unexplored_tiles = path.bfs(grid, robot_pos, find_unidentified=True) if unexplored_tiles and len(path.a_star(grid, robot_pos, unexplored_tiles[0])) > 7: state = TRAVEL continue st.set_speed(AUTO_MODE, 50, 'tower') # Wait for new laser values while 0 < num_laser_values.value < 520 and num_hall_reads.value == 0: pass if laser_counter < mapper.maxiter: #print('NOT ENOUGH READS, UPDATING MAP') # Copy laser sensor reads from shared array if there are less then 520 reads laser_list = [] with laser_values.get_lock(): if num_laser_values.value > 520: with num_laser_values.get_lock(): num_laser_values.value = 0 continue laser_list = [laser_values[i] for i in range(num_laser_values.value)] # Update map based on the new sensor values if laser_list: laser_counter += 1 # Generate submap generated_map = slammer.mapper(laser_list) # Add the submap to the 'final' one mapper.add_submap(generated_map[0], generated_map[1], (int(robot_pos[0]), int(robot_pos[1])), robot_dir) fingrid = [[UNKNOWN_TILE for x in range(MAP_SIZE)] for y in range(MAP_SIZE)] for y in range(MAP_SIZE): for x in range(MAP_SIZE): fingrid[y][x] = get_grid_value(x,y,grid) mapper.generate_map(fingrid) elif laser_counter >= mapper.maxiter: if (not first_laser_read): first_laser_read = True print('\nENOUGH LASER READS, STOPPING TOWER', laser_counter) laser_counter = 0 stop_tower(num_hall_reads) raycast_map = [[0 for i in range(MAP_SIZE)] for j in range(MAP_SIZE)] for y in range(31): for x in range(31): value = 0 if (mapper.submaps): value = float(mapper.finalmap[y][x]) / len(mapper.submaps) tile_value = UNKNOWN_TILE if (value >= 0.5): tile_value = WALL_TILE raycast_map[y][x] = tile_value subp = sm.slice_submap(ir_grid, (int(robot_pos[0]), int(robot_pos[1]))) raycast_map = sm.returnFinal(raycast_map, subp[0], subp[1], (int(robot_pos[0]), int(robot_pos[1]))) ray_floor = sm.raycast_floor(raycast_map,(int(robot_pos[0]), int(robot_pos[1]))) for y in range(31): for x in range(31): ray_val = raycast_map[y][x] #las_val = laser_floor[y][x] rayflorval = ray_floor[y][x] if (rayflorval == 1): laser_floor[y][x] = rayflorval las_val = laser_floor[y][x] if (las_val == 1): if (ray_val < 2): raycast_map[y][x] = 1 for i in range(31): for j in range(31): value = raycast_map[i][j] if (value >= 2): value = 2 set_grid_value(j, i, value, grid) mapper.remove_bad_values() if path.closed_room(grid, robot_pos): # Find closest path home print('ROOM IS CLOSED, GO HOME') path_home = path.a_star(grid, robot_pos, [15.5, 15.5]) travel_commands = path.follow_path(path_home, robot_dir) print("Path home: " + str(travel_commands)) state = TRAVEL ir_enabled = False else: print('ROOM IS NOT CLOSED, ENTER TRAVEL') state = TRAVEL #state = WALL continue # Executes commands sent from the laptop elif state == MANUAL: st.set_speed(AUTO_MODE, 0) prev_state = MANUAL stop_tower(num_hall_reads) time.sleep(1) # Enter autonomous mode if mode.value == 1: state = START continue # Disable pid and start recieving commands pid.enabled = False st.steering_serial(steering_cmd_man, mode, ir_values) # Blocks st.steering_port.flushInput() st.forward() continue