Waiter_group/route1.py

240 lines
8.9 KiB
Python
Raw Normal View History

2020-04-26 22:49:41 +02:00
import pygame
import numpy as np
import math
# Colors:
# Define some colors
BLACK = (0, 0, 0)
WHITE = (255, 255, 255)
GREEN = (0, 255, 0)
RED = (255, 0, 0)
BLUE = (0, 0, 240)
#Width and Height of each square:
WIDTH = 20
HEIGHT = 20
#Margin:
MARGIN = 5
grid = [[0 for x in range(16)] for y in range(16)]
def change_value(i, j, width, n):
for r in range (i, i+width):
for c in range (j, j+width):
grid[r][c] = n
class Table:
def __init__(self, coordinate_i, coordinate_j):
self.coordinate_i = coordinate_i
self.coordinate_j = coordinate_j
change_value(coordinate_i, coordinate_j, 2, 1)
def get_destination_coor(self):
return [self.coordinate_i, self.coordinate_j-1]
class Kitchen:
def __init__(self, coordinate_i, coordinate_j):
self.coordinate_i = coordinate_i
self.coordinate_j = coordinate_j
change_value(coordinate_i, coordinate_j, 3, 2)
class Agent:
def __init__(self,orig_coordinate_i, orig_coordinate_j):
self.orig_coordinate_i = orig_coordinate_i
self.orig_coordinate_j = orig_coordinate_j
self.state = np.array([1,2])
change_value(orig_coordinate_j, orig_coordinate_j, 1, 3)
self.state_update(orig_coordinate_i, orig_coordinate_j)
self.previous_grid = np.array([-1, -1])
def state_update(self, c1, c2):
self.state[0] = c1
self.state[1] = c2
def leave(self):
change_value(self.state[0], self.state[1], 1, 0)
def previous_grid_update(self):
self.previous_grid[0] = self.state[0]
self.previous_grid[1] = self.state[1]
def move_to(self, nextPos):
self.previous_grid_update()
self.leave()
self.state_update(x + nextPos[0], y + nextPos[1])
change_value(self.state[0], self.state[1], 1, 3)
# check the next grid is not the previous grid to prevent the loop
def next_is_previous(self, x, y):
return np.array_equal(self.previous_grid, np.array([x, y]))
def check_done():
for event in pygame.event.get(): # Checking for the event
if event.type == pygame.QUIT: # If the program is closed:
return True # To exit the loop
def draw_grid():
for row in range(16): # Drawing the grid
for column in range(16):
color = WHITE
if grid[row][column] == 1:
color = GREEN
if grid[row][column] == 2:
color = RED
if grid[row][column] == 3:
color = BLUE
pygame.draw.rect(screen,
color,
[(MARGIN + WIDTH) * column + MARGIN,
(MARGIN + HEIGHT) * row + MARGIN,
WIDTH,
HEIGHT])
# calculate the distance between two points
def distance(point1, point2):
return math.sqrt((point2[0] - point1[0])**2 + (point2[1] - point1[1])**2)
## default positions of the agent:
x = 12
y = 12
agent = Agent(x, y)
table1 = Table(2, 2)
table2 = Table (2,7)
table3 = Table(2, 12)
table4 = Table(7, 2)
table5 = Table(7, 7)
table6 = Table(7, 12)
table7 = Table(12, 2)
table8 = Table(12, 7)
#class Kitchen:
kitchen = Kitchen(13, 13)
# destination array
destination_tables = []
destination_tables.append(table1.get_destination_coor()) # append [2,1]
destination_tables.append(table2.get_destination_coor()) # append [2,6]
destination_tables.append(table3.get_destination_coor()) # append [2,11]
destination_tables.append(table4.get_destination_coor()) # append [7,1]
destination_tables.append(table5.get_destination_coor()) # append [7,6]
destination_tables.append(table6.get_destination_coor()) # append [7,11]
destination_tables.append(table7.get_destination_coor()) # append [12,1]
destination_tables.append(table8.get_destination_coor()) # append [12,6]
pygame.init()
WINDOW_SIZE = [405, 405]
screen = pygame.display.set_mode(WINDOW_SIZE)
pygame.display.set_caption("Waiter_Grid3")
done = False
clock = pygame.time.Clock()
# -------- Main Program Loop -----------
while not done:
screen.fill(BLACK) # Background color
draw_grid()
done = check_done()
while len(destination_tables) != 0:
# set the first element(table) in array as currDestination
currDestination = destination_tables[0]
# from kitchen to table
while agent.state[0] != currDestination[0] or agent.state[1] != currDestination[1]:
#///////////////////////////////////////
x = agent.state[0]
y = agent.state[1]
# set a huge default number
minDis = 9999
nextPos = []
# check whether the agent goes left
if y-1 >= 0 and grid[x][y-1] != 1 and not agent.next_is_previous(x, y-1):
minDis = distance([x, y-1], currDestination)
nextPos = [0, -1] # means go left
# check whether the agent goes right
if y+1 <= 15 and grid[x][y+1] != 1 and grid[x][y+1] != 2 and not agent.next_is_previous(x, y+1):
d = distance([x, y+1], currDestination)
if d < minDis:
minDis = d
nextPos = [0, 1] # means go right
# check whether the agent goes up
if x-1 >= 0 and grid[x-1][y] != 1 and not agent.next_is_previous(x-1, y):
d = distance([x-1, y], currDestination)
if d < minDis:
minDis = d
nextPos = [-1, 0] # means go up
# check whether the agent goes down
if x+1 <= 15 and grid[x+1][y] != 1 and grid[x+1][y] != 2 and not agent.next_is_previous(x+1, y):
d = distance([x+1, y], currDestination)
if d < minDis:
minDis = d
nextPos = [1, 0] # means go down
print(agent.previous_grid)
agent.move_to(nextPos)
#////////////////////////////////////////////////
pygame.time.delay(100)
screen.fill(BLACK) # Background color
draw_grid() # Drawing the grid
clock.tick(60) # Limit to 60 frames per second
pygame.display.flip() # Updating the screen
# set the kitchen as currDestination
currDestination = [13, 12]
# from table to kitchen
while agent.state[0] != currDestination[0] or agent.state[1] != currDestination[1]:
#///////////////////////////////////////
x = agent.state[0]
y = agent.state[1]
# set a huge default number
minDis = 9999
nextPos = []
# check whether the agent goes left
if y-1 >= 0 and grid[x][y-1] != 1 and not agent.next_is_previous(x, y-1):
minDis = distance([x, y-1], currDestination)
nextPos = [0, -1] # means go left
# check whether the agent goes right
if y+1 <= 15 and grid[x][y+1] != 1 and grid[x][y+1] != 2 and not agent.next_is_previous(x, y+1):
d = distance([x, y+1], currDestination)
if d < minDis:
minDis = d
nextPos = [0, 1] # means go right
# check whether the agent goes up
if x-1 >= 0 and grid[x-1][y] != 1 and grid[x-1][y] != 2 and not agent.next_is_previous(x-1, y):
d = distance([x-1, y], currDestination)
if d < minDis:
minDis = d
nextPos = [-1, 0] # means go up
# check whether the agent goes down
if x+1 <= 15 and grid[x+1][y] != 1 and grid[x+1][y] != 2 and not agent.next_is_previous(x+1, y):
d = distance([x+1, y], currDestination)
if d < minDis:
minDis = d
nextPos = [1, 0] # means go down
agent.move_to(nextPos)
#////////////////////////////////////////////////
pygame.time.delay(100)
screen.fill(BLACK) # Background color
draw_grid() # Drawing the grid
clock.tick(60) # Limit to 60 frames per second
pygame.display.flip() # Updating the screen
destination_tables = destination_tables[1:] # remove the first element in the list
pygame.quit()