erftgrf

route1.py

@@ -0,0 +1,240 @@
+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()