From cf83f07f79646952ab7b0ae882d542432f3341d0 Mon Sep 17 00:00:00 2001 From: Fabio Manganiello Date: Mon, 26 Feb 2018 11:31:06 +0100 Subject: [PATCH] Implemented auto pilot with distance sensors --- platypush/plugins/gpio/sensor/__init__.py | 0 .../plugins/gpio/sensor/distance/__init__.py | 99 +++++++++++++++++++ platypush/plugins/gpio/zeroborg/__init__.py | 43 +++++++- 3 files changed, 141 insertions(+), 1 deletion(-) create mode 100644 platypush/plugins/gpio/sensor/__init__.py create mode 100644 platypush/plugins/gpio/sensor/distance/__init__.py diff --git a/platypush/plugins/gpio/sensor/__init__.py b/platypush/plugins/gpio/sensor/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/platypush/plugins/gpio/sensor/distance/__init__.py b/platypush/plugins/gpio/sensor/distance/__init__.py new file mode 100644 index 00000000..b528ee12 --- /dev/null +++ b/platypush/plugins/gpio/sensor/distance/__init__.py @@ -0,0 +1,99 @@ +import logging +import threading +import time + +import RPi.GPIO as gpio + +from platypush.message.response import Response +from platypush.plugins import Plugin + + +class GpioSensorDistancePlugin(Plugin): + def __init__(self, trigger_pin, echo_pin): + self.trigger_pin = trigger_pin + self.echo_pin = echo_pin + self.measured_distance_mm = None + self._is_probing = False + + gpio.setmode(gpio.BCM) + gpio.setup(self.trigger_pin, gpio.OUT) + gpio.setup(self.echo_pin, gpio.IN) + + logging.info('Resetting trigger sensor on GPIO pin {}'.format(self.trigger_pin)) + gpio.output(self.trigger_pin, False) + time.sleep(1) + + + def start(self): + self._is_probing = True + logging.info('Received distance sensor start command') + + def _run(): + gpio.setmode(gpio.BCM) + gpio.setup(self.trigger_pin, gpio.OUT) + gpio.setup(self.echo_pin, gpio.IN) + + while self._is_probing: + gpio.output(self.trigger_pin, True) + 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 + + while gpio.input(self.echo_pin) == 0: + pulse_on = time.time() + if pulse_on - pulse_start > 0.5: + logging.warning('Distance sensor echo timeout: 0') + read_timeout = True + break + + pulse_start = pulse_on + pulse_end = time.time() + pulse_off = pulse_end + + while gpio.input(self.echo_pin) == 1: + pulse_off = time.time() + if pulse_off - pulse_end > 0.5: + logging.warning('Distance sensor echo timeout: 1') + read_timeout = True + break + + if read_timeout: + continue + + pulse_end = pulse_off + pulse_duration = pulse_end - pulse_start + + # s = vt where v = avg speed of sound + self.measured_distance_mm = round(pulse_duration * 171500.0, 2) + + gpio.output(self.trigger_pin, False) + time.sleep(0.1) + + gpio.cleanup() + self.measured_distance_mm = None + + self._probe_thread = threading.Thread(target=_run) + self._probe_thread.start() + + return Response(output={'status': 'probing'}) + + + def get_distance(self): + return self.measured_distance_mm + + + def stop(self): + self._is_probing = False + self.measured_distance_mm = None + + if self._probe_thread and threading.get_ident() != self._probe_thread.ident: + self._probe_thread.join() + + return Response(output={'status':'stopped'}) + + +# vim:sw=4:ts=4:et: + diff --git a/platypush/plugins/gpio/zeroborg/__init__.py b/platypush/plugins/gpio/zeroborg/__init__.py index c8bc63a5..5c26cf1a 100644 --- a/platypush/plugins/gpio/zeroborg/__init__.py +++ b/platypush/plugins/gpio/zeroborg/__init__.py @@ -5,6 +5,8 @@ import time from platypush.message.response import Response from platypush.plugins import Plugin +from platypush.context import get_plugin +from platypush.config import Config class Direction(enum.Enum): @@ -12,6 +14,7 @@ class Direction(enum.Enum): DIR_DOWN = 'down' DIR_LEFT = 'left' DIR_RIGHT = 'right' + DIR_AUTO = 'auto' class GpioZeroborgPlugin(Plugin): @@ -37,6 +40,8 @@ class GpioZeroborgPlugin(Plugin): self.v_in = v_in self.v_out = v_out self.max_power = v_out / float(v_in) + self.distance_sensor = None + self.auto_mode = False self.zb = ZeroBorg.ZeroBorg() self.zb.Init() @@ -44,6 +49,19 @@ class GpioZeroborgPlugin(Plugin): self.zb.ResetEpo() + def get_distance(self): + if not self.distance_sensor: + self.distance_sensor = get_plugin('gpio.sensor.distance') + + if not self.distance_sensor: + raise RuntimeError('No gpio.sensor.distance configuration found') + + if self.distance_sensor.get_distance() is None: + self.distance_sensor.start() + + return self.distance_sensor.get_distance() + + def drive(self, direction): self._can_run = True self._direction = direction.lower() @@ -54,6 +72,22 @@ class GpioZeroborgPlugin(Plugin): left = 0.0 right = 0.0 + if self._direction == Direction.DIR_AUTO.value: + self.auto_mode = True + + if self.auto_mode: + distance = None + while distance is None: + distance = self.get_distance() + + print(distance) + if distance > 200.0: # distance in mm + self._direction = Direction.DIR_UP.value + else: + self._direction = Direction.DIR_LEFT.value + + time.sleep(0.1) + if self._direction == Direction.DIR_UP.value: left = 1.0 + self._power_offsets[Direction.DIR_LEFT][Direction.DIR_UP] right = 1.0 + self._power_offsets[Direction.DIR_RIGHT][Direction.DIR_UP] @@ -65,15 +99,18 @@ class GpioZeroborgPlugin(Plugin): right = -1.25 - self._power_offsets[Direction.DIR_RIGHT][Direction.DIR_DOWN] elif self._direction == Direction.DIR_RIGHT.value: left = -1.25 - self._power_offsets[Direction.DIR_LEFT][Direction.DIR_DOWN] - right = 2 + self._power_offsets[Direction.DIR_RIGHT][Direction.DIR_UP] + right = 2.0 + self._power_offsets[Direction.DIR_RIGHT][Direction.DIR_UP] elif self._direction is not None: logging.warning('Invalid direction: {}'.format(direction)) + self.stop() self.zb.SetMotor1(left * self.max_power) self.zb.SetMotor2(left * self.max_power) self.zb.SetMotor3(-right * self.max_power) self.zb.SetMotor4(-right * self.max_power) + self.auto_mode = False + self._drive_thread = threading.Thread(target=_run) self._drive_thread.start() @@ -88,6 +125,10 @@ class GpioZeroborgPlugin(Plugin): self.zb.MotorsOff() self.zb.ResetEpo() + + if self.distance_sensor: + self.distance_sensor.stop() + return Response(output={'status':'stopped'})