101 lines
3.9 KiB
Python
101 lines
3.9 KiB
Python
import pygame
|
|
import bfs
|
|
import random
|
|
|
|
from num_map import num_matrix
|
|
from pygame.locals import *
|
|
from Forklift import Forklift
|
|
|
|
HORIZONTAL = 1250
|
|
VERTICAL = 750
|
|
|
|
TILE_SIZE = 50
|
|
|
|
## 4x6x5
|
|
def handle_turn(initial_state, turn_count):
|
|
if turn_count % 2 == 0:
|
|
commands = bfs.graphsearch(initial_state=initial_state, goal_list=(7, 23))
|
|
else:
|
|
row = random.randint(0, 1) * 10 + 2 * random.randint(0, 2)
|
|
col = random.randint(1, 5) + 6 * random.randint(0, 3)
|
|
commands = bfs.graphsearch(initial_state=initial_state, goal_list=(row, col))
|
|
print('I need to go to row: ' + str(row) + ' column: ' + str(col))
|
|
return commands
|
|
|
|
|
|
class Program:
|
|
def __init__(self):
|
|
pygame.init()
|
|
self.window = pygame.display.set_mode((HORIZONTAL, VERTICAL))
|
|
self.caption = pygame.display.set_caption('Autonomiczny wózek widłowy')
|
|
self.agent = Forklift(self.window)
|
|
self.agent.drawForklift()
|
|
|
|
running = True
|
|
|
|
currRow = 7
|
|
currCol = 1
|
|
currDirec = 1
|
|
|
|
turn_count = 0
|
|
|
|
initial_state = bfs.State(row=int(self.agent.y/50), column=int(self.agent.x/50), direction=self.agent.direction)
|
|
|
|
while running:
|
|
initial_state.row = int(self.agent.y/50)
|
|
initial_state.column = int(self.agent.x/50)
|
|
initial_state.direction = self.agent.direction
|
|
|
|
commands = handle_turn(initial_state=initial_state, turn_count=turn_count)
|
|
|
|
for event in pygame.event.get():
|
|
if event.type == KEYDOWN:
|
|
if event.key == K_RIGHT:
|
|
if currCol + 1 < 25 and num_matrix[currRow][currCol + 1] == 0:
|
|
num_matrix[currRow][currCol] = 0
|
|
currCol = currCol + 1
|
|
num_matrix[currRow][currCol] = -1
|
|
self.agent.moveForkliftRight()
|
|
if event.key == K_LEFT:
|
|
if currCol - 1 > -1 and num_matrix[currRow][currCol - 1] == 0:
|
|
num_matrix[currRow][currCol] = 0
|
|
currCol = currCol - 1
|
|
num_matrix[currRow][currCol] = -1
|
|
self.agent.moveForkliftLeft()
|
|
if event.key == K_UP:
|
|
if currRow - 1 > -1 and num_matrix[currRow - 1][currCol] == 0:
|
|
num_matrix[currRow][currCol] = 0
|
|
currRow -= 1
|
|
num_matrix[currRow][currCol] = -1
|
|
self.agent.moveForkliftUp()
|
|
if event.key == K_DOWN:
|
|
if currRow + 1 < 15 and num_matrix[currRow + 1][currCol] == 0:
|
|
num_matrix[currRow][currCol] = 0
|
|
currRow += 1
|
|
num_matrix[currRow][currCol] = -1
|
|
self.agent.moveForkliftDown()
|
|
if event.key == K_ESCAPE:
|
|
running = False
|
|
elif event.type == pygame.QUIT:
|
|
running = False
|
|
|
|
while commands:
|
|
pygame.event.poll()
|
|
step = commands.pop(0)
|
|
if step == 'rotate_left':
|
|
self.agent.rotate_forklift_left()
|
|
currDirec = self.agent.direction
|
|
elif step == 'rotate_right':
|
|
self.agent.rotate_forklift_right()
|
|
currDirec = self.agent.direction
|
|
elif step == 'go':
|
|
self.agent.forklift_move()
|
|
currRow = int(self.agent.y/50)
|
|
currCol = int(self.agent.x/50)
|
|
pygame.time.delay(250)
|
|
print('row: ' + str(currRow) + ' col: ' + str(currCol))
|
|
print(commands)
|
|
turn_count += 1
|
|
print(turn_count)
|
|
pygame.time.delay(2000)
|