Modularized the (already existing) odometry class. Added some handy functions to reset loop control condition variables. Added second bumper to sensor class.

src/move.py

@@ -22,10 +22,8 @@ class Move:
             self._sensor = Sensor()
         except OSError:
             print("ERROR: Cannot find Motor/Sensor")
-            self.maxbrightness = 0
+        self.defaultspeed = 20
-            self.minbrightness = 0
+        self._odometry = Odometry()
-            self.edgebrightness = 0
-            self._odometry = Odometry()
 
     '''
     determine maximum and minimum brightness of lines/white space
@@ -36,31 +34,7 @@ class Move:
         self.calibrateBrightness()
         # pass
 
-    def calibrateBrightness(self):
+
-        i = 0
-        self.maxbrightness = self._sensor.getbrightness()
-        self.minbrightness = self._sensor.getbrightness()
-        self._wheel_l.speed_set(15)
-        self._wheel_r.speed_set(-15)
-        self._wheel_r.run()
-        self._wheel_l.run()
-        tempvalue = 0
-        while(i < 500):
-            self._sensor.refresh()
-            tempvalue = self._sensor.getbrightness()
-            if(tempvalue > self.maxbrightness):
-                self.maxbrightness = tempvalue
-            if(tempvalue < self.minbrightness):
-                self.minbrightness = tempvalue
-            i = i + 1
-        self._wheel_l.stop()
-        self._wheel_r.stop()
-        self.edgebrightness = (self.maxbrightness + self.minbrightness)//2
-        while(not (self._sensor.getbrightness() == self.edgebrightness)):
-            self._wheel_l.speed_set(15)
-            self._wheel_r.speed_set(-15)
-        self._wheel_l.stop()
-        self._wheel_r.stop()
 
     '''
     Function to correct errors should the robot wander astray
@@ -77,22 +51,26 @@ class Move:
     Next turn around and detect edges through drops in _sensor.getbrightness()
     '''
     def getstationedges(self):
-        self._wheel_l.turnbyamount(500, 250)
+        pass
-        self._wheel_r.turnbyamount(-500, 250)
+
+    def turnto(self, direction):
+        turnval = self._odometry.dir2ticks(direction)
+        self._wheel_l.turnbyamount(turnval, self.defaultspeed)
+        self._wheel_r.turnbyamount(-turnval, self.defaultspeed)
 
     def traversetonextstation(self, isknownstation):
         self._wheel_l.speed_set(self.edgebrightness//2)
         self._wheel_r.speed_set(self.edgebrightness//2)
         self._wheel_l.run()
         self._wheel_r.run()
-
+        while (not self._sensor.bumperwaspressed and self._sensor.lastcolor is None):
-        while (self._sensor.bumperispressed() is not True and self._sensor.lastcolor is None):
+            self._sensor.refresh()
-            self._camera.refresh()
+            self._sensor.checkbumper()
-            self._wheel_l.speed_set(capat100(self.maxbrightness/2 - self._sensor.getbrightness()/2))
+            self._wheel_l.speed_set(capat100(self.maxbrightness/2 - self._sensor.getbrightness()/2)+self.defaultspeed)
-            self._wheel_r.speed_set(capat100(self._sensor.getbrightness()/2))
+            self._wheel_r.speed_set(capat100(self._sensor.getbrightness()/2 - self.maxbrightness/2)+self.defaultspeed)
             self._sensor.getcolor()
             if(not isknownstation):
-                print(self._odometry.degree(self._wheel_l.getmovement(), self._wheel_r.getmovement(), 0))
+                self._odometry.pos_update((self._wheel_l.getmovement(), self._wheel_r.getmovement()))
 
         self._wheel_l.stop()
         self._wheel_r.stop()

src/odometry.py

@@ -5,6 +5,7 @@
 import time
 from planet import Direction
 import math
+import decimal
 
 '''
 Documentation:
@@ -16,7 +17,7 @@ axis a (measure distance where wheels have most friction)
 calibration factor c = diameter/ticks_per_turn
 distance d = ticks wheel * c (calculate for each wheel individually)
 vector_length = (d_l + d_r)/2
-vector_angle ~= (d_l + d_r)/a      (only applicable for small distances)
+vector_angle ~= (d_l - d_r)/a      (only applicable for small distances)
 
 Functionality that should be implemented in this class:
 - tracking of relative distances
@@ -26,37 +27,55 @@ Functionality that should be implemented in this class:
 '''
 
 
+def angle2dir(angle):
+    if angle % (2 * math.pi) >= 0 and angle % (2 * math.pi) < math.pi * 0.25 or angle % (2 * math.pi) >= 7 / 4 * math.pi and angle % (2 * math.pi) < 2 * math.pi:
+        return Direction.NORTH
+    elif angle % (2 * math.pi) >= math.pi * 0.25 and angle % (2 * math.pi) < math.pi * 0.5 or angle % (2 * math.pi) >= math.pi * 0.5 and angle % (2 * math.pi) < math.pi * 0.75:
+        return Direction.EAST
+    elif angle % (2 * math.pi) >= math.pi * 0.75 and angle % (2 * math.pi) < math.pi or angle % (2 * math.pi) >= math.pi and angle % (2 * math.pi) < 5 / 4 * math.pi:
+        return Direction.SOUTH
+    else:
+        return Direction.WEST
+
+
+def posround(pos):
+    return decimal.Decimal(pos/50).quantize(0, rounding=decimal.ROUND_HALF_UP)
+
+
 class Odometry:
     def __init__(self):  # planet
         self._wheel_axis = 11
         self._distance_per_tick = 0.0488
-        self._pi = 3.141
+        self._pi = math.pi
-        self.distance = 0
+        self._posxy = [0, 0]
-        self.alpha = 0
+        self._distance = [0, 0]
+        self._v_length = 0
+        self._v_angle = Direction.NORTH
 
+    def reset(self):
+        self._distance = [0, 0]
+        self._v_length = 0
+        self._posxy = [0, 0]
 
-    # worked fine outside the class as function in nano, maybe we must fix it a bit :)
+    def angle_set(self, newangle):
-    def degree(self, delta_rotation_l, delta_rotation_r):  # delta_rotation_l/r should work with getmovement method
+        self._v_angle = newangle
 
-        self.alpha = ((delta_rotation_l * self._distance_per_tick - delta_rotation_r * self._distance_per_tick)/self._wheel_axis) + self.alpha
+    def coordinates_get(self):
-        self.distance = (delta_rotation_r * self._distance_per_tick + delta_rotation_l * self._distance_per_tick)/2 + self.distance
+        return ((posround(self._posxy[0]), posround(self._posxy[1])), angle2dir(self._v_angle))
-        if self.alpha % (2 * self._pi) >= 0 and self.alpha % (2 * self._pi) < self._pi * 0.25 or self.alpha % (2 * self._pi) >= 7 / 4 * self._pi and self.alpha % (2 * self._pi) < 2 * self._pi:
-            return(Direction.NORTH, "Distance = ", self.distance)
 
-        elif self.alpha % (2 * self._pi) >= self._pi * 0.25 and self.alpha % (2 * self._pi) < self._pi * 0.5 or self.alpha % (2 * self._pi) >= self._pi * 0.5 and self.alpha % (2 * self._pi) < self._pi * 0.75:
+    def pos_update(self, ticks_wheel):
-            return(Direction.EAST, "Distance = ", self.distance)
+        self._distance[0] = ticks_wheel[0] * self._distance_per_tick
+        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 + (ticks_wheel[0] - ticks_wheel[1])/self._wheel_axis
+        self.coordinates_update()
+        print((self._v_length, angle2dir(self._v_angle)))
 
-        elif self.alpha % (2 * self._pi) >= self._pi * 0.75 and self.alpha % (2 * self._pi) < self._pi or self.alpha % (2 * self._pi) >= self._pi and self.alpha % (2 * self._pi) < 5 / 4 * self._pi:
+    def coordinates_update(self):  # worked as function outside the class
-            return(Direction.SOUTH, "Distance = ", self.distance)
+        self._posxy[0] = self._posxy[0] + self._v_length * math.sin(self._v_angle)
+        self._posxy[1] = self._posxy[1] + self._v_length * math.cos(self._v_angle)
+        print((self._posxy[0], self._posxy[1]))
 
-        else:
+    def dir2ticks(destdir):  # return amount of ticks to turn to a given direction (current direction should be _v_angle)
-            return(Direction.WEST, "Distance = ", self.distance)
+        ticksperwheel = None
-
+        return ticksperwheel
-    def coordinates(self, delta_rota_l, delta_rota_r, Y_koord, X_koord):  # worked as function outside the class
-        self.y = self.Y_koord
-        self.x = self.X_koord
-        self.wheel_center = (delta_rota_r + delta_rota_l) / 2
-        self.alpha = ((self.delta_rotation_l * self._distance_per_tick - self.delta_rotation_r * self._distance_per_tick)/self._wheel_axis) + self.alpha
-        self.y = self.y + self.wheel_center * math.cos(self.alpha)
-        self.x = self.x + self.wheel_center * math.sin(self.alpha)
-        return(self.x, self.y)

src/sensor.py

@@ -24,25 +24,40 @@ Y=(0.299 x R) + (0.587 x G) + (0.114 x B);
 
 
 class Color(tuple, Enum):
-    RED = (90, 40, 25)
+    RED = (85, 40, 25)
-    BLUE = (25, 75, 90)
+    BLUE = (25, 70, 85)
 
 
 class Sensor:
     def __init__(self):
-        self._bumper = ev3.TouchSensor()
+        try:
-        self._camera = ev3.ColorSensor()
+            self._bumper_l = ev3.TouchSensor("in1")
-        self._camera.mode = 'RGB-RAW'
+            self._bumper_r = ev3.TouchSensor("in4")
+            self.bumperwaspressed = False
+            self._camera = ev3.ColorSensor()
+            self._camera.mode = 'RGB-RAW'
+            self.RGB = self._camera.bin_data("hhh")
+            self._brightness = self._camera.value()
+        except OSError:
+            print("Error while setting up sensors. Please make sure that everything is plugged in properly.")
+        self.lastcolor = None
+        self.edgebrightness = None
+
+    def refresh(self):  # do we still want to check if the propper camera/sensor mode is set?
         self.RGB = self._camera.bin_data("hhh")
         self._brightness = self._camera.value()
+
+    def reset(self):
+        self.bumperwaspressed = False
         self.lastcolor = None
 
-    def bumperispressed(self):
+    def calibrateBrightness(self):
-        self._bumper.value()
+        self.refresh()
+        self.edgebrightness = self.getbrightness()
 
-    def refresh(self): # do we still want to check if the propper camera/sensor mode is set?
+    def checkbumper(self):
-        self.RGB = self._camera.bin_data("hhh")
+        if(self._bumper_l.value() or self._bumper_r.value()):
-        self._brightness = self._camera.value()
+            self.bumperwaspressed = True
 
     def getbrightness(self):
         return self._brightness
@@ -55,5 +70,4 @@ class Sensor:
                 self.lastcolor = Color.BLUE
 
     def calibrateRGB(self):
-        # Enums are unchangeable right? Dunno how to calibrate like that
         pass