diff options
| -rw-r--r-- | gui.py | 30 | ||||
| -rw-r--r-- | main.py | 51 | ||||
| -rw-r--r-- | maps/exampleRobotMap | 27 |
3 files changed, 101 insertions, 7 deletions
@@ -1,4 +1,5 @@ -import pygame, sys +import pygame +import sys from pygame.locals import * pygame.init() @@ -6,21 +7,40 @@ pygame.init() BLACK = (0, 0, 0) GRAY = (99, 99, 99) WHITE = (255, 255, 255) -WINDOW_HEIGHT = 400 -WINDOW_WIDTH = 400 +WINDOW_HEIGHT = 800 +WINDOW_WIDTH = 800 MARGIN = 20 -GRID_HEIGHT = 5 -GRID_WIDTH = 4 +MATRIX_HEIGHT = 5 +MATRIX_WIDTH = 4 +square_size = 100 + + +GridWidthpx = (MATRIX_WIDTH * square_size) + (MATRIX_WIDTH * square_size // 10) +GridHeightpx = (MATRIX_HEIGHT * square_size) + (MATRIX_HEIGHT * square_size // 10) +square_x = GridWidthpx // 4 - square_size // 4 +square_y = GridHeightpx // 4 - square_size // 4 + +square_x_orig = square_x displaySurface = pygame.display.set_mode((WINDOW_WIDTH, WINDOW_HEIGHT), 0, 32) pygame.display.set_caption('tourob GUI') displaySurface.fill(WHITE) +for y in range(MATRIX_HEIGHT): + square_x = square_x_orig + pygame.draw.rect(displaySurface, BLACK, (square_x, square_y, MARGIN, square_size)) + for x in range(MATRIX_WIDTH): + pygame.draw.rect(displaySurface, BLACK, (square_x - MARGIN, square_y, MARGIN, square_size)) + pygame.draw.rect(displaySurface, GRAY, (square_x, square_y, square_size, square_size)) + square_x += square_size + MARGIN + pygame.draw.rect(displaySurface, BLACK, (square_x - MARGIN, square_y, MARGIN, square_size)) + square_y += square_size + MARGIN while True: for event in pygame.event.get(): if event.type == QUIT: pygame.quit() sys.exit() + pygame.display.update() @@ -69,17 +69,64 @@ def getShortestPath(courseMap, startPos, endPos): visitedCoordinates += nextPos +def move(direction): + # if direction = 'right': + # moving = True + # while moving: + # Motors.moving etc + # when motors.stop: + # moving = False + # if direction = 'left': + # moving = True + # while moving: + # Motors.moving etc + # when motors.stop: + # moving = False + # if direction = 'right': + # moving = True + # while moving: + # Motors.moving etc + # when motors.stop: + # moving = False + # if direction = 'up': + # moving = True + # while moving: + # Motors.moving etc + # when motors.stop: + # moving = False + return 0 + + def solveCourse(course, obstacles, checkpoints): + stepsLength = 0 + for x in range(len(checkpoints) - 1): shortestPath = getShortestPath(course, checkpoints[x], checkpoints[x + 1]) - if x != len(checkpoints) - 2: - del shortestPath[-1] + # if x != 0: + # del shortestPath[0] + for i in range(len(shortestPath) - 1): + diffX = shortestPath[i + 1][0] - shortestPath[i][0] + if diffX == 1: + move('right') + elif diffX == -1: + move('left') + + diffY = shortestPath[i + 1][1] - shortestPath[i][1] + if diffY == 1: + move('down') + elif diffY == -1: + move('up') + + stepsLength += 1 print(shortestPath) + print("Amount of steps: %d" % (stepsLength)) populateCourse(courseMap, MAPWIDTH, MAPHEIGHT) + startTime = time.time() solveCourse(courseMap, wallList, checkpointList) endTime = time.time() executionTime = endTime - startTime + print("Execution Time: %.4f seconds" % executionTime) diff --git a/maps/exampleRobotMap b/maps/exampleRobotMap new file mode 100644 index 0000000..940d0ef --- /dev/null +++ b/maps/exampleRobotMap @@ -0,0 +1,27 @@ +MAPWIDTH = 4 +MAPHEIGHT = 5 + +STARTPOINT = [0, 2] +ENDPOINT = [1, 4] + +checkpointList = [ + STARTPOINT, + [0, 0], + [1, 4], + [3, 2], + [3, 4], + ENDPOINT + ] + +wallList = + [[2, 0], [3, 0]], + [[1, 0], [1, 1]], + [[3, 0], [3, 1]], + [[0, 1], [0, 2]], + [[0, 2], [1, 2]], + [[2, 2], [3, 2]], + [[1, 3], [1, 4]], + [[3, 3], [3, 4]], + [[1, 4], [2, 4]] + ] + |