From bdfe48fe32a3cbfa70992cae1819b718b8333bfc Mon Sep 17 00:00:00 2001 From: Fabio Manganiello Date: Sat, 21 Dec 2019 13:09:44 +0100 Subject: [PATCH] Refactored integration between ZeroBorg and distance sensor plugin --- platypush/plugins/gpio/sensor/__init__.py | 4 + .../plugins/gpio/sensor/distance/__init__.py | 87 ++++++++++++------- platypush/plugins/gpio/zeroborg/__init__.py | 64 ++++++++------ 3 files changed, 98 insertions(+), 57 deletions(-) diff --git a/platypush/plugins/gpio/sensor/__init__.py b/platypush/plugins/gpio/sensor/__init__.py index c747b14d..6ab5c432 100644 --- a/platypush/plugins/gpio/sensor/__init__.py +++ b/platypush/plugins/gpio/sensor/__init__.py @@ -36,5 +36,9 @@ class GpioSensorPlugin(Plugin): return self.get_measurement(*args, **kwargs).output + @action + def close(self): + pass + # vim:sw=4:ts=4:et: diff --git a/platypush/plugins/gpio/sensor/distance/__init__.py b/platypush/plugins/gpio/sensor/distance/__init__.py index 48ddf553..2855a0f1 100644 --- a/platypush/plugins/gpio/sensor/distance/__init__.py +++ b/platypush/plugins/gpio/sensor/distance/__init__.py @@ -15,67 +15,76 @@ class GpioSensorDistancePlugin(GpioPlugin, GpioSensorPlugin): * ``RPi.GPIO`` (``pip install RPi.GPIO``) """ - def __init__(self, trigger_pin, echo_pin, *args, **kwargs): + def __init__(self, trigger_pin: int, echo_pin: int, + timeout: float = 1.0, warmup_time: float = 2.0, *args, **kwargs): """ - :param trigger_pin: GPIO PIN where you connected your sensor trigger PIN (the one that triggers the sensor to perform a measurement). - :type trigger_pin: int + :param trigger_pin: GPIO PIN where you connected your sensor trigger PIN (the one that triggers the + sensor to perform a measurement). - :param echo_pin: GPIO PIN where you connected your sensor echo PIN (the one that will listen for the signal to bounce back and therefore trigger the distance calculation). - :type echo_pin: int + :param echo_pin: GPIO PIN where you connected your sensor echo PIN (the one that will listen for the + signal to bounce back and therefore trigger the distance calculation). + + :param timeout: The echo-wait will terminate and the plugin will return null if no echo has been + received after this time (default: 1 second). + + :param warmup_time: Number of seconds that should be waited on plugin instantiation + for the sensor to be ready (default: 2 seconds). """ GpioPlugin.__init__(self, *args, **kwargs) self.trigger_pin = trigger_pin self.echo_pin = echo_pin - self._is_probing = False + self.timeout = timeout + self.warmup_time = warmup_time + self._initialized = False + self._init_gpio() def _init_gpio(self): + if self._initialized: + return + import RPi.GPIO as gpio gpio.setmode(self.mode) gpio.setup(self.trigger_pin, gpio.OUT) gpio.setup(self.echo_pin, gpio.IN) - gpio.output(self.trigger_pin, False) + gpio.output(self.trigger_pin, gpio.LOW) + + self.logger.info('Waiting {} seconds for the sensor to be ready'.format(self.warmup_time)) + time.sleep(self.warmup_time) + self.logger.info('Sensor ready') + self._initialized = True def _get_data(self): import RPi.GPIO as gpio + + pulse_start = pulse_end = pulse_on = pulse_off = time.time() + self._init_gpio() - - gpio.output(self.trigger_pin, True) + gpio.output(self.trigger_pin, gpio.HIGH) time.sleep(0.00001) # 1 us pulse to trigger echo measurement - gpio.output(self.trigger_pin, False) - read_timeout = False - - pulse_start = time.time() - pulse_on = pulse_start + gpio.output(self.trigger_pin, gpio.LOW) while gpio.input(self.echo_pin) == 0: pulse_on = time.time() - if pulse_on - pulse_start > 0.5: - self.logger.warning('Distance sensor echo timeout: 0') - read_timeout = True - break + if pulse_on - pulse_start > self.timeout: + raise TimeoutError('Distance sensor echo timeout after {} seconds: 0'. + format(self.timeout)) pulse_start = pulse_on - pulse_end = time.time() - pulse_off = pulse_end + pulse_end = pulse_off = time.time() while gpio.input(self.echo_pin) == 1: pulse_off = time.time() - if pulse_off - pulse_end > 0.5: - self.logger.warning('Distance sensor echo timeout: 1') - read_timeout = True - break - - if read_timeout: - return None + if pulse_off - pulse_end > self.timeout: + raise TimeoutError('Distance sensor echo timeout after {} seconds: 1'. + format(self.timeout)) pulse_end = pulse_off pulse_duration = pulse_end - pulse_start - # s = vt where v = avg speed of sound + # s = vt where v = 1/2 * avg speed of sound in mm/s return round(pulse_duration * 171500.0, 2) - @action def get_measurement(self): """ @@ -83,12 +92,28 @@ class GpioSensorDistancePlugin(GpioPlugin, GpioSensorPlugin): :returns: Distance measurement as a scalar (in mm): """ - import RPi.GPIO as gpio try: return self._get_data() - finally: + except TimeoutError as e: + self.logger.warning(str(e)) + return + except Exception as e: + self.close() + raise e + + @action + def close(self): + import RPi.GPIO as gpio + if self._initialized: gpio.cleanup() + self._initialized = False + + def __enter__(self): + self._init_gpio() + + def __exit__(self): + self.close() # vim:sw=4:ts=4:et: diff --git a/platypush/plugins/gpio/zeroborg/__init__.py b/platypush/plugins/gpio/zeroborg/__init__.py index 5a0d036d..a51c2294 100644 --- a/platypush/plugins/gpio/zeroborg/__init__.py +++ b/platypush/plugins/gpio/zeroborg/__init__.py @@ -129,6 +129,10 @@ class GpioZeroborgPlugin(Plugin): value = self._get_measurement(plugin=plugin, timeout=sensor['timeout']) threshold = sensor['threshold'] + if value is None: + self.logger.warning('Read timeout from sensor {}'.format(sensor['plugin'])) + return Direction.DIR_AUTO.value + if value >= threshold and 'above_threshold_direction' in sensor: direction = sensor['above_threshold_direction'] elif 'below_threshold_direction' in sensor: @@ -156,37 +160,43 @@ class GpioZeroborgPlugin(Plugin): self.logger.info('Received ZeroBorg drive command: {}'.format(direction)) def _run(): - while self._can_run and self._direction: - if self._direction == Direction.DIR_AUTO_TOGGLE.value: - if self.auto_mode: - self._direction = None - self.auto_mode = False - else: - self._direction = Direction.DIR_AUTO + try: + while self._can_run and self._direction: + if self._direction == Direction.DIR_AUTO_TOGGLE.value: + if self.auto_mode: + self._direction = None + self.auto_mode = False + else: + self._direction = Direction.DIR_AUTO + self.auto_mode = True + + if self._direction == Direction.DIR_AUTO.value: self.auto_mode = True - if self._direction == Direction.DIR_AUTO.value: - self.auto_mode = True + motor_1_power = motor_2_power = motor_3_power = motor_4_power = 0.0 - if self.auto_mode: - self._direction = self._get_direction_from_sensors() - time.sleep(0.1) + try: + if self.auto_mode: + self._direction = self._get_direction_from_sensors() + time.sleep(0.1) - motor_1_power = motor_2_power = motor_3_power = motor_4_power = 0.0 - if self._direction in self.directions: - motor_1_power = self.directions[self._direction]['motor_1_power'] - motor_2_power = self.directions[self._direction]['motor_2_power'] - motor_3_power = self.directions[self._direction]['motor_3_power'] - motor_4_power = self.directions[self._direction]['motor_4_power'] - elif self._direction: - self.logger.warning('Invalid direction {}, stopping motors'.format(self._direction)) + if self._direction in self.directions and self._direction != Direction.DIR_AUTO.value: + motor_1_power = self.directions[self._direction]['motor_1_power'] + motor_2_power = self.directions[self._direction]['motor_2_power'] + motor_3_power = self.directions[self._direction]['motor_3_power'] + motor_4_power = self.directions[self._direction]['motor_4_power'] + else: + self.logger.warning('Invalid direction {}: stopping motors'.format(self._direction)) + except Exception as e: + self.logger.error('Error on _get_direction_from_sensors: {}'.format(str(e))) + break - self.zb.SetMotor1(motor_1_power) - self.zb.SetMotor2(motor_2_power) - self.zb.SetMotor3(motor_3_power) - self.zb.SetMotor4(motor_4_power) - - self.auto_mode = False + self.zb.SetMotor1(motor_1_power) + self.zb.SetMotor2(motor_2_power) + self.zb.SetMotor3(motor_3_power) + self.zb.SetMotor4(motor_4_power) + finally: + self.stop() self._drive_thread = threading.Thread(target=_run) self._drive_thread.start() @@ -200,6 +210,8 @@ class GpioZeroborgPlugin(Plugin): """ self._can_run = False + self.auto_mode = False + if self._drive_thread and threading.get_ident() != self._drive_thread.ident: self._drive_thread.join()