graphserach v0.6.3

FindPath.py

@@ -58,12 +58,13 @@ def graphsearch(tractor, board, TillageUnit, fringe: Queue, explored):
         elem = fringe.get()
 
         if goaltest(elem, end_state):
+            break
             #TODO
-            return #droga ktora musi pokonac traktor
+            #return droga ktora musi pokonac traktor
-
+        else:
             explored.append(elem)
-
+            elem = succ(start_state, end_state, tractor)
-
+            fringe.put(elem)
 
 
 def goaltest(elem, end_state):
@@ -72,6 +73,31 @@ def goaltest(elem, end_state):
     else:
         return False
 
-def succ():
+def succ(start_state, end_state, tractor):
-    #TODO
+    if start_state[1] < end_state[1]:
-    return
+        if tractor.direction == "RIGHT":
+            tractor.drive()
+        else:
+            tractor.direction = "RIGHT"
+        print("przesunalem sie w prawo")
+    elif start_state[1] > end_state[1]:
+        if tractor.direction == "LEFT":
+            tractor.drive()
+        else:
+            tractor.direction = "LEFT"
+        print("przesunalem sie w lewo")
+    else:
+        if start_state[0] < end_state[0]:
+            if tractor.direction == "DOWN":
+                tractor.drive()
+            else:
+                tractor.direction = "DOWN"
+            print("przesunalem sie w dol")
+        elif start_state[0] > end_state[0]:
+            if tractor.direction == "UP":
+                tractor.drive()
+            else:
+                tractor.direction = "UP"
+            print("przesunalem sie w gore")
+        else:
+            return tractor.horizontal_index, tractor.horizontal_index