Added maps dir and example map, added step counter and added steps back. Added pseudocode to main.py

3 files changed, 101 insertions, 7 deletions

diff --git a/gui.py b/gui.py
index 2a2cdde..3f9400c 100644
--- a/gui.py
+++ b/gui.py
@@ -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()
diff --git a/main.py b/main.py
index 9da2b37..097d8bd 100644
--- a/main.py
+++ b/main.py
@@ -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]]
+        ]
+