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
