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_MIDDLE_PIN): GPIO.read(BOTTOM_MIDDLE_PIN),
-        str(BOTTOM_RIGHT_PIN):  GPIO.read(BOTTOM_RIGHT_PIN)}
+    str(BOTTOM_LEFT_PIN): GPIO.read(BOTTOM_LEFT_PIN),
+    str(BOTTOM_MIDDLE_PIN): GPIO.read(BOTTOM_MIDDLE_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
-        spi.writebytes([channel << 3, 0])
-        bytes = spi.readbytes(2)
-        return (bytes[0] * 256 + bytes[1]) >> 2
+        return analog_states[str(BOTTOM_LEFT_CHANNEL)]
 
     @staticmethod
     def bottom_middle_analog() -> int:
@@ -113,10 +144,7 @@ class IRSensor(object):
         :return: 10-bit brightness value
         :rtype: int
         """
-        channel = 1
-        spi.writebytes([channel << 3, 0])
-        bytes = spi.readbytes(2)
-        return (bytes[0] * 256 + bytes[1]) >> 2
+        return analog_states[str(BOTTOM_MIDDLE_CHANNEL)]
 
     @staticmethod
     def bottom_right_analog() -> int:
@@ -125,7 +153,5 @@ class IRSensor(object):
         :return: 10-bit brightness value
         :rtype: int
         """
-        channel = 0
-        spi.writebytes([channel << 3, 0])
-        bytes = spi.readbytes(2)
-        return (bytes[0] * 256 + bytes[1]) >> 2
+        return analog_states[str(BOTTOM_RIGHT_CHANNEL)]
+

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())