Add IRWrapper and change IRSensor logic

compLib/IRSensor.py

@@ -3,6 +3,8 @@ import pigpio
 from compLib.ADC import ADC
 from compLib.LogstashLogging import Logging
 import spidev
+import threading
+import time
 
 TOP_LEFT_CHANNEL = 0
 TOP_RIGHT_CHANNEL = 1
@@ -14,6 +16,10 @@ BOTTOM_LEFT_PIN = 14
 BOTTOM_MIDDLE_PIN = 15
 BOTTOM_RIGHT_PIN = 23
 
+BOTTOM_LEFT_CHANNEL = 2
+BOTTOM_MIDDLE_CHANNEL = 1
+BOTTOM_RIGHT_CHANNEL = 0
+
 adc = ADC()
 spi = spidev.SpiDev()
 spi.open(0, 0)
@@ -28,18 +34,46 @@ GPIO.set_mode(BOTTOM_MIDDLE_PIN, pigpio.INPUT)
 GPIO.set_mode(BOTTOM_RIGHT_PIN, pigpio.INPUT)
 
 states = {
-        str(BOTTOM_LEFT_PIN): GPIO.read(BOTTOM_LEFT_PIN),
+    str(BOTTOM_LEFT_PIN): GPIO.read(BOTTOM_LEFT_PIN),
-        str(BOTTOM_MIDDLE_PIN): GPIO.read(BOTTOM_MIDDLE_PIN),
+    str(BOTTOM_MIDDLE_PIN): GPIO.read(BOTTOM_MIDDLE_PIN),
-        str(BOTTOM_RIGHT_PIN):  GPIO.read(BOTTOM_RIGHT_PIN)}
+    str(BOTTOM_RIGHT_PIN): GPIO.read(BOTTOM_RIGHT_PIN)}
+
+analog_states = {
+    str(BOTTOM_LEFT_CHANNEL): 0,
+    str(BOTTOM_MIDDLE_CHANNEL): 0,
+    str(BOTTOM_RIGHT_CHANNEL): 0}
+
 
 def gpio_callback(gpio, level, tick):
     states[str(gpio)] = level
-#    print(gpio, level, tick)
+
 
 GPIO.callback(BOTTOM_LEFT_PIN, pigpio.EITHER_EDGE, gpio_callback)
 GPIO.callback(BOTTOM_MIDDLE_PIN, pigpio.EITHER_EDGE, gpio_callback)
 GPIO.callback(BOTTOM_RIGHT_PIN, pigpio.EITHER_EDGE, gpio_callback)
 
+
+def read_analog_channel(channel):
+    spi.writebytes([channel << 3, 0])
+
+    time.sleep(0.0033)  # 300HZ
+
+    bytes = spi.readbytes(2)
+    return (bytes[0] * 256 + bytes[1]) >> 2
+
+
+def sensor_thread():
+    while True:
+        analog_states[str(BOTTOM_LEFT_CHANNEL)] = read_analog_channel(BOTTOM_LEFT_CHANNEL)
+        analog_states[str(BOTTOM_MIDDLE_CHANNEL)] = read_analog_channel(BOTTOM_MIDDLE_CHANNEL)
+        analog_states[str(BOTTOM_RIGHT_CHANNEL)] = read_analog_channel(BOTTOM_RIGHT_CHANNEL)
+
+
+analog_thread = threading.Thread(target=sensor_thread, daemon=True)
+analog_thread.start()
+time.sleep(0.25)
+
+
 class IRSensor(object):
     """Access the different IR Sensors at top / bottom of the robot
     """
@@ -101,10 +135,7 @@ class IRSensor(object):
         :return: 10-bit brightness value
         :rtype: int
         """
-        channel = 2
+        return analog_states[str(BOTTOM_LEFT_CHANNEL)]
-        spi.writebytes([channel << 3, 0])
-        bytes = spi.readbytes(2)
-        return (bytes[0] * 256 + bytes[1]) >> 2
 
     @staticmethod
     def bottom_middle_analog() -> int:
@@ -113,10 +144,7 @@ class IRSensor(object):
         :return: 10-bit brightness value
         :rtype: int
         """
-        channel = 1
+        return analog_states[str(BOTTOM_MIDDLE_CHANNEL)]
-        spi.writebytes([channel << 3, 0])
-        bytes = spi.readbytes(2)
-        return (bytes[0] * 256 + bytes[1]) >> 2
 
     @staticmethod
     def bottom_right_analog() -> int:
@@ -125,7 +153,5 @@ class IRSensor(object):
         :return: 10-bit brightness value
         :rtype: int
         """
-        channel = 0
+        return analog_states[str(BOTTOM_RIGHT_CHANNEL)]
-        spi.writebytes([channel << 3, 0])
+
-        bytes = spi.readbytes(2)
-        return (bytes[0] * 256 + bytes[1]) >> 2

compLib/IRWrapper.py

@@ -0,0 +1,81 @@
+from compLib.IRSensor import IRSensor
+
+import time
+
+ir = IRSensor()
+
+MIN_VALUE = 0.0
+MAX_VALUE = 0.0
+MOVING_AVERAGE_SIZE = 30.0
+
+
+def approximate_rolling_average(avg, value):
+    avg -= avg / MOVING_AVERAGE_SIZE
+    avg += value / MOVING_AVERAGE_SIZE
+    return avg
+
+
+class IRWrapper(object):
+    """Wrapper around the IRSensor to enable calibration of the sensor
+    """
+
+    @staticmethod
+    def calibrate():
+        """Calibrate the black and white values of the IRSensors
+        This is done by putting the bot on black line with the middle sensor
+        Afterwards, all sensors are read for 2 seconds and filtered.
+        The minimum value is used for white, maximum is black.
+        """
+        global MIN_VALUE
+        global MAX_VALUE
+
+        left_avg = ir.bottom_left_analog()
+        middle_avg = ir.bottom_middle_analog()
+        right_avg = ir.bottom_right_analog()
+
+        start_time = time.time()
+        while time.time() - start_time < 2:
+            left_avg = approximate_rolling_average(left_avg, ir.bottom_left_analog())
+            middle_avg = approximate_rolling_average(middle_avg, ir.bottom_middle_analog())
+            right_avg = approximate_rolling_average(right_avg, ir.bottom_right_analog())
+            time.sleep(0.01)
+
+        MIN_VALUE = min([left_avg, middle_avg, right_avg])
+        MAX_VALUE = max([left_avg, middle_avg, right_avg])
+
+    @staticmethod
+    def adjust_for_calibration(raw_value: float) -> float:
+        """Adjust a raw sensor value to 0 to 1000
+
+        :return: Value between 0 and 1000, White and Black
+        :rtype: float
+        """
+        x = (raw_value - MIN_VALUE) * 1000.0 / (MAX_VALUE - MIN_VALUE)
+        return max(min(1000.0, x), 0.0)
+
+    @staticmethod
+    def bottom_left_calibrated() -> float:
+        """Returns calibrated value of the bottom left analog sensor
+
+        :return: Value between 0 and 1000, White and Black
+        :rtype: float
+        """
+        return IRWrapper.adjust_for_calibration(ir.bottom_left_analog())
+
+    @staticmethod
+    def bottom_middle_calibrated() -> float:
+        """Returns calibrated value of the bottom middle analog sensor
+
+        :return: Value between 0 and 1000, White and Black
+        :rtype: float
+        """
+        return IRWrapper.adjust_for_calibration(ir.bottom_middle_analog())
+
+    @staticmethod
+    def bottom_right_calibrated() -> float:
+        """Returns calibrated value of the bottom right analog sensor
+
+        :return: Value between 0 and 1000, White and Black
+        :rtype: float
+        """
+        return IRWrapper.adjust_for_calibration(ir.bottom_right_analog())

docs/source/conf.py

@@ -35,7 +35,7 @@ extensions = [
     'sphinx_rtd_theme'
 ]
 
-autodoc_mock_imports = ["smbus", "compLib.PCA9685", "RPi", "pigpio", "flask"]
+autodoc_mock_imports = ["smbus", "compLib.PCA9685", "RPi", "pigpio", "flask", "apt", "spidev"]
 
 # Add any paths that contain templates here, relative to this directory.
 templates_path = ['_templates']

docs/source/lib/IRWrapper.rst

@@ -0,0 +1,22 @@
+.. _lib_irwrapper:
+
+Infrared Wrapper
+****************
+
+.. autoclass:: compLib.IRWrapper.IRWrapper
+   :members:
+
+Examples
+=========
+
+Calibrating analog sensors
+--------------------------
+
+.. code-block:: python
+
+   from compLib import IRWrapper
+
+   ir = IRWrapper.IRWrapper()
+
+    ir.calibrate()
+    print("left {} middle {} right {}".format(ir.bottom_left_calibrated(), ir.bottom_middle_calibrated(), ir.bottom_right_calibrated()))

docs/source/lib/Linefollower.rst

@@ -0,0 +1,89 @@
+.. _lib_Linefollower:
+
+Linefollower Examples
+*********************
+
+Simple Linefollower
+-------------------------
+
+.. code-block:: python
+
+    import time
+
+    from IRWrapper import IRWrapper
+    from compLib.Motor import Motor
+
+    STRAIGHT_SPEED = 40.0
+    TURN_SPEED = 40.0
+
+    COLOR_BREAK = 500
+
+
+    def drive(left, right, active_break=False, back_mult=0.0):
+        left = max(min(left, 100), -100)
+        right = max(min(right, 100), -100)
+
+        print("left {} right {}".format(left, right))
+
+        Motor.power(0, left)
+        Motor.power(1, left * back_mult)
+        Motor.power(2, right * back_mult)
+        Motor.power(3, right)
+
+        if active_break:
+            time.sleep(0.001)
+
+            Motor.power(0, 10)
+            Motor.power(1, 10)
+            Motor.power(2, 10)
+            Motor.power(3, 10)
+
+            time.sleep(0.001)
+
+
+    def follow_till_line():
+        while True:
+            print("left: {:.2f}  middle: {:.2f}  right: {:.2f}".format(IRWrapper.bottom_left_calibrated(),
+                                                                       IRWrapper.bottom_middle_calibrated(),
+                                                                       IRWrapper.bottom_right_calibrated()))
+
+            left = IRWrapper.bottom_left_calibrated()
+            right = IRWrapper.bottom_right_calibrated()
+
+            if left > COLOR_BREAK:
+                drive(-TURN_SPEED, TURN_SPEED, back_mult=1.25)
+                while IRWrapper.bottom_middle_calibrated() < COLOR_BREAK and \
+                        IRWrapper.bottom_right_calibrated() < COLOR_BREAK:
+                    pass
+                drive(0, 0)
+            elif right > COLOR_BREAK:
+                drive(TURN_SPEED, -TURN_SPEED, back_mult=1.25)
+                while IRWrapper.bottom_middle_calibrated() < COLOR_BREAK and \
+                        IRWrapper.bottom_left_calibrated() < COLOR_BREAK:
+                    pass
+                drive(0, 0)
+            else:
+                drive(STRAIGHT_SPEED, STRAIGHT_SPEED, True)
+
+            if IRWrapper.bottom_left_calibrated() > 750 and IRWrapper.bottom_right_calibrated() > 750:
+                break
+
+        print("left: {}  middle: {}  right: {}".format(IRWrapper.bottom_left_calibrated(),
+                                                       IRWrapper.bottom_middle_calibrated(),
+                                                       IRWrapper.bottom_right_calibrated()))
+
+        drive(0, 0)
+
+        time.sleep(0.2)
+
+
+    if __name__ == "__main__":
+        # Put bot with middle sensor onto black line
+        IRWrapper.calibrate()
+
+        # Initialize Motors, so the bot doesn't speed off instantly
+        drive(1, 1)
+        time.sleep(1)
+
+        # Follow line, till the left and right sensor are on black
+        follow_till_line()