From e94d735e24277b47c3ea37e1f10376a1b3d78891 Mon Sep 17 00:00:00 2001 From: Konstantin Lampalzer Date: Sun, 28 Mar 2021 16:39:54 +0200 Subject: [PATCH] Add IRWrapper and change IRSensor logic --- compLib/IRSensor.py | 58 +++++++++++++++------ compLib/IRWrapper.py | 81 +++++++++++++++++++++++++++++ docs/source/conf.py | 2 +- docs/source/lib/IRWrapper.rst | 22 ++++++++ docs/source/lib/Linefollower.rst | 89 ++++++++++++++++++++++++++++++++ 5 files changed, 235 insertions(+), 17 deletions(-) create mode 100644 compLib/IRWrapper.py create mode 100644 docs/source/lib/IRWrapper.rst create mode 100644 docs/source/lib/Linefollower.rst diff --git a/compLib/IRSensor.py b/compLib/IRSensor.py index d98b6dd..69463fa 100644 --- a/compLib/IRSensor.py +++ b/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 \ No newline at end of file + return analog_states[str(BOTTOM_RIGHT_CHANNEL)] + diff --git a/compLib/IRWrapper.py b/compLib/IRWrapper.py new file mode 100644 index 0000000..dd18206 --- /dev/null +++ b/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()) diff --git a/docs/source/conf.py b/docs/source/conf.py index a455a50..5c77524 100644 --- a/docs/source/conf.py +++ b/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'] diff --git a/docs/source/lib/IRWrapper.rst b/docs/source/lib/IRWrapper.rst new file mode 100644 index 0000000..15232a7 --- /dev/null +++ b/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())) diff --git a/docs/source/lib/Linefollower.rst b/docs/source/lib/Linefollower.rst new file mode 100644 index 0000000..407d349 --- /dev/null +++ b/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()