More work towards a working main loop...

src/communication.py

@@ -103,8 +103,14 @@ class Communication:
     def status(self):
         return self._status
 
-    def update(self, edges):  # TODO: FINISH
+    def update(self, odometry, edges):  # TODO: FINISH
+        if(odometry != None):
+            self.planet.setcurnode(self.planet.getcurnode() + odometry[0])
         if(edges is None and self._status is Mode.EXPLORE):
+            if(self.planet.unexedge == []):
+                self.encode_message(Command.COMPLETE, None)
+            else:
+                self.navto = next(filter(lambda x: x is not None, map(lambda edge: self.planet.shortest_path(self.planet.getcurnode(), edge), self.planet.unexedge)))
                 self._status = Mode.GOTOSTATION
         elif(edges is None):
             pass
@@ -145,8 +151,9 @@ class Communication:
         elif message.startswith(Command.PATH):
             self.stringtopath(*message[5:].rsplit(' '))
         elif message.startswith(Command.TARGET):
-            self._status = Mode.TARGET
             self.navto = self.planet.shortest_path(self.planet.getcurnode(), str2tuple(*message[7:].rsplit(',')))
+            if(self.navto != None):
+                self._status = Mode.TARGET
         elif message.startswith(Command.NOTICE):
             print(message)
 

src/main.py

@@ -27,13 +27,14 @@ def run():
         while(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:
                 for instructions in communication.navto:
-                    communication.update(None)
+                    communication.update(None, None)
                     move.turnto(instructions[1])
                     move.traversetonextstation(True)
 
-        communication.update(move.getstationedges())  # TODO: Register in map
+        communication.update(move._odometry.coordinates_get(), move.getstationedges())  # TODO: Register in map
+        move.updatevars()
         move.turnto(communication.navto[0][1])  # TODO: add control mechanism to communication class
-        if(communication.check(move.traversetonextstation(False)) is False):  # TODO: let same control mechanism decide if station is known, also traversetonextstation needs a return value
+        if(communication.check(move.traversetonextstation(False)) is False):
             move.setcurdir(communication.getdir())
 
 

src/move.py

@@ -3,6 +3,8 @@
 from wheel import Wheel
 from sensor import Sensor
 from odometry import Odometry
+from planet import Direction
+from time import sleep
 
 
 def capat100(value):
@@ -35,14 +37,22 @@ class Move:
         self._sensor.calibrateBrightness()
         # pass
 
-
-
     '''
     Function to correct errors should the robot wander astray
     '''
 
     def _aligntoedge(self):
-        pass
+        self._wheel_l.speed_set(20)
+        self._wheel_r.speed_set(-20)
+        self._wheel_l.run()
+        self._wheel_r.run()
+        self._sensor.refresh()
+        while(self._sensor.getbrightness() > self._sensor.edgebrightness):
+            self._sensor.refresh()
+        self._wheel_l.stop()
+        self._wheel_r.stop()
+        self._wheel_l.turnbyamount(20, 20)
+        self._wheel_r.turnbyamount(-20, 20)
 
     '''
     uses odometry and the planet object to map edges that are connected to
@@ -52,12 +62,25 @@ class Move:
     Next turn around and detect edges through drops in _sensor.getbrightness()
     '''
     def getstationedges(self):
-        pass
+        found_edges = []
+        self._odometry.reset()
+        while(True):
+            self._aligntoedge()
+            self._odometry.pos_update((self._wheel_l.getmovement(), self._wheel_r.getmovement()))
+            if(self._odometry.angle_get() in found_edges):
+                break
+            else:
+                found_edges.append(self._odometry.angle_get())
+                self._wheel_l.turnbyamount(20, 20)
+                self._wheel_r.turnbyamount(-20, 20)
+                sleep(2)
+        return found_edges
+
 
     def turnto(self, direction):
         turnval = self._odometry.dir2ticks(direction)
-        self._wheel_l.turnbyamount(turnval, self.defaultspeed)
-        self._wheel_r.turnbyamount(-turnval, self.defaultspeed)
+        self._wheel_l.turnbyamount(turnval, 90)
+        self._wheel_r.turnbyamount(-turnval, 90)
 
     def traversetonextstation(self, isknownstation):
         self._wheel_l.speed_set(self._sensor.edgebrightness//2)
@@ -72,6 +95,5 @@ class Move:
             self._sensor.getcolor()
             if(not isknownstation):
                 self._odometry.pos_update((self._wheel_l.getmovement(), self._wheel_r.getmovement()))
-
         self._wheel_l.stop()
         self._wheel_r.stop()

src/odometry.py

@@ -61,6 +61,9 @@ class Odometry:
     def angle_set(self, newangle):
         self._v_angle = newangle
 
+    def angle_get(self):
+        return angle2dir(self._v_angle)
+
     def coordinates_get(self):
         return ((posround(self._posxy[0]), posround(self._posxy[1])), angle2dir(self._v_angle))
 
@@ -69,8 +72,8 @@ class Odometry:
         self._distance[1] = ticks_wheel[1] * self._distance_per_tick
         self._v_length = self._v_length + (self._distance[0] + self._distance[1])/2
         self._v_angle = self._v_angle + (self._distance[0] - self._distance[1])/self._wheel_axis
-        self.coordinates_update()
         self._wheel_center = (self._distance[0] + self._distance[1])/2
+        self.coordinates_update()
         print((self._v_length, angle2dir(self._v_angle)))
 
     def coordinates_update(self):  # worked as function outside the class
@@ -79,10 +82,6 @@ class Odometry:
         print((self._posxy[0], self._posxy[1]))
 
     def dir2ticks(self, destdir):  # return amount of ticks to turn to a given direction (current direction should be _v_angle)
-        self._ticksperwheel = 0
-        self._current_Direc = (self._v_angle /self._pi) * 180
-        self._ticks_def = self._current_Direc - destdir
-        self._ticksperwheel = self._ticks_def / 2
-        if self._ticksperwheel < 0:
-            self._ticksperwheel = - self._ticksperwheel
-        return ticksperwheel
+        turnval = (destdir - self._v_angle) * 2
+        self._v_angle = destdir
+        return turnval

src/odomtest.py

@@ -0,0 +1,19 @@
+from move import Move
+
+mymove = Move()
+mymove._sensor.refresh()
+mymove.maxbrightness = mymove._sensor.getbrightness()
+mymove.maxbrightness = 50
+mymove._wheel_l.getmovement()
+mymove._wheel_r.getmovement()
+
+
+
+try:
+    mymove.traversetonextstation(False)
+except:
+    mymove._wheel_l.stop()
+    mymove._wheel_r.stop()
+    mymove._wheel_l.getmovement()
+    mymove._wheel_r.getmovement()
+

src/planet.py

@@ -50,7 +50,7 @@ class Planet:
     def __init__(self):
         """ Initializes the data structure """
         self._planetmap = {}
-        self._unexedge = []
+        self.unexedge = []
         self._curnode = None
         self._curdir = Direction.NORTH
         self.target = None
@@ -79,10 +79,10 @@ class Planet:
         return self._curdir
 
     def addtounexedge(self, node_edge_list):
-        self._unexedge = self._unexedge + node_edge_list
+        self.unexedge = self.unexedge + node_edge_list
 
     def exploreedge(self, node_edge):
-        self._unexedge.remove(node_edge)
+        self.unexedge.remove(node_edge)
 
     '''
     Returns a shortest path between two nodes.