Now it kind of works? There are still some bugs to squash

src/communication.py

@@ -108,11 +108,12 @@ class Communication:
 
     def update(self, odometry):  # TODO: FINISH
         if(self.isfirstrun):
-            pass
+            self.isfirstrun = False
         elif(odometry is not None):  # use odometry to send data
             self.encode_message(Command.PATH, ((self.planet.getcurnode(), self.planet.getcurdir()), ((self.planet.getcurnode()[0] + odometry[0][0], self.planet.getcurnode()[1] + odometry[0][1]), (odometry[1] - 180) % 360), odometry[2]))
         elif(self._status is Mode.GOTOSTATION or self._status is Mode.TARGET):
             self.encode_message(Command.PATH, ((self.navto[0][0], self.navto[0][1]), (self.planet._planetmap[self.navto[0][0]][self.navto[0][1]][0], self.planet._planetmap[self.navto[0][0]][self.navto[0][1]][1]), self.planet._planetmap[self.navto[0][0]][self.navto[0][1]][2]))
+            # self.encode_message(Command.PATH, ((self.navto[0][0], self.navto[0][1]),        (self.planet._planetmap[self.navto[0][0]][self.navto[0][1]][0], self.planet._planetmap[self.navto[0][0]][self.navto[0][1]][1]), self.planet._planetmap[self.navto[0][0]][self.navto[0][1]][2]))
         self.process_messages()  # this should update curnode and curdir
 
     def process_edges(self, edges):

src/main.py

@@ -24,20 +24,21 @@ def run():
     while(communication.planetname is None):
         communication.process_messages()
 
-    move.traversetonextstation(False)
-
+    move.traversetonextstation(True)
+    move.resetwheels()
+    move.reset()
     while(communication.status() is not Mode.COMPLETE):
         communication.update(move.getstats())  # TODO: Register in map
         # correct odometry right here otherwise list of edges will be incorrect
         move.setcurdir(communication.getdir())
         communication.process_edges(move.getstationedges())
         if(communication.status() is Mode.GOTOSTATION or communication.status() is Mode.TARGET):  # should be triggered if current station has no unexplored edges
-            for instructions in communication.navto:
-                move.turnto(instructions[1])
+            while len(communication.navto) > 0:
+                move.turnto(communication.navto[0][1])
                 move.traversetonextstation(True)
-                move.setcurdir(communication.planet._planetmap[instructions[0]][instructions[1]][1])
+                move.setcurdir(communication.planet._planetmap[communication.navto[0][0]][communication.navto[0][1]])
                 communication.update(None)
-            communication.navto.clear()
+                communication.navto.pop(0)
             communication._status = Mode.EXPLORE
         else:
             move.turnto(communication.navto[0][1])

src/move.py

@@ -124,6 +124,10 @@ class Move:
         self._odometry.reset()
         self._sensor.reset()
 
+    def resetwheels(self):
+        self._wheel_l._motor.position = 0
+        self._wheel_r._motor.position = 0
+
     def traversetonextstation(self, isknownstation):
         self._wheel_l.speed_set(self._sensor.edgebrightness//2)
         self._wheel_r.speed_set(self._sensor.edgebrightness//2)

src/planet.py

@@ -89,7 +89,7 @@ class Planet:
                 testnode.remove(edges)
         if(testnode != [] and self._curnode not in self.unexedge):
             self.unexedge.append(self._curnode)
-        if(testnode == [] and self._curnode in self.unexedge):
+        if(len(testnode) <= 1 and self._curnode in self.unexedge):
             self.unexedge.remove(self._curnode)
         print("MAP:", self._planetmap[self._curnode])
         print("NODES:", testnode)