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

1 files changed, 49 insertions, 2 deletions

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)