From b863213bd799b07f9be42c7d8749e2af3a7c83ed Mon Sep 17 00:00:00 2001
From: Fabio Manganiello <blacklight86@gmail.com>
Date: Fri, 13 Apr 2018 15:49:10 +0200
Subject: [PATCH] More rational management of ZeroBorg auto pilot mode together
 with sensor - no longer limited to gpio.sensor.distance alone, but including
 any kind of GpioSensorPlugin. Thresholds, timeouts and actions are configured
 in config.yaml instead of being hardcoded

---
 platypush/plugins/gpio/sensor/__init__.py     | 13 ++++
 .../plugins/gpio/sensor/distance/__init__.py  | 11 ++--
 platypush/plugins/gpio/zeroborg/__init__.py   | 61 ++++++++++---------
 3 files changed, 51 insertions(+), 34 deletions(-)

diff --git a/platypush/plugins/gpio/sensor/__init__.py b/platypush/plugins/gpio/sensor/__init__.py
index e69de29bb2..b6abbdb050 100644
--- a/platypush/plugins/gpio/sensor/__init__.py
+++ b/platypush/plugins/gpio/sensor/__init__.py
@@ -0,0 +1,13 @@
+from platypush.plugins import Plugin
+
+
+class GpioSensorPlugin(Plugin):
+    def __init__(self, *args, **kwargs):
+        super().__init__(*args, **kwargs)
+
+    def get_measurement(self, *args, **kwargs):
+        raise NotImplementedError('get_measurement should be implemented in a derived class')
+
+
+# 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 e61f5403f5..d7ecfb06c3 100644
--- a/platypush/plugins/gpio/sensor/distance/__init__.py
+++ b/platypush/plugins/gpio/sensor/distance/__init__.py
@@ -5,11 +5,13 @@ import time
 import RPi.GPIO as gpio
 
 from platypush.message.response import Response
-from platypush.plugins import Plugin
+from platypush.plugins.gpio.sensor import GpioSensorPlugin
 
 
-class GpioSensorDistancePlugin(Plugin):
-    def __init__(self, trigger_pin, echo_pin):
+class GpioSensorDistancePlugin(GpioSensorPlugin):
+    def __init__(self, trigger_pin, echo_pin, *args, **kwargs):
+        super().__init__(*args, **kwargs)
+
         self.trigger_pin = trigger_pin
         self.echo_pin = echo_pin
         self._is_probing = False
@@ -20,10 +22,9 @@ class GpioSensorDistancePlugin(Plugin):
 
         logging.info('Resetting trigger sensor on GPIO pin {}'.format(self.trigger_pin))
         gpio.output(self.trigger_pin, False)
-        time.sleep(1)
 
 
-    def get_distance(self):
+    def get_measurement(self):
         gpio.setmode(gpio.BCM)
         gpio.setup(self.trigger_pin, gpio.OUT)
         gpio.setup(self.echo_pin, gpio.IN)
diff --git a/platypush/plugins/gpio/zeroborg/__init__.py b/platypush/plugins/gpio/zeroborg/__init__.py
index 4a4f25cdd3..8b9c50affd 100644
--- a/platypush/plugins/gpio/zeroborg/__init__.py
+++ b/platypush/plugins/gpio/zeroborg/__init__.py
@@ -37,12 +37,38 @@ class GpioZeroborgPlugin(Plugin):
         self.zb.ResetEpo()
 
 
-    def get_distance(self):
-        distance_sensor = get_plugin('gpio.sensor.distance')
-        if not distance_sensor:
-            raise RuntimeError('No gpio.sensor.distance configuration found')
+    def _get_measurement(self, plugin, timeout):
+        measure_start_time = time.time()
+        value = None
 
-        return distance_sensor.get_distance()
+        while value is None:
+            value = plugin.get_measurement()
+            if time.time() - measure_start_time > timeout:
+                return None
+
+        return value
+
+    def _get_direction_from_sensors(self):
+        if Direction.DIR_AUTO.value not in self.directions:
+            raise RuntimeError("Can't start auto pilot: " +
+                               "no sensor configured in gpio.zeroborg.directions.auto")
+
+        direction = None
+
+        for sensor in self.directions[Direction.DIR_AUTO.value]['sensors']:
+            plugin = get_plugin(sensor['plugin'])
+            if not sensor:
+                raise RuntimeError('No such sensor: ' + sensor['plugin'])
+
+            value = self._get_measurement(plugin=plugin, timeout=sensor['timeout'])
+            threshold = sensor['threshold']
+
+            if value >= threshold and 'above_threshold_direction' in sensor:
+                direction = sensor['above_threshold_direction']
+            elif 'below_threshold_direction' in sensor:
+                direction = sensor['below_threshold_direction']
+
+        return direction
 
 
     def drive(self, direction):
@@ -69,30 +95,7 @@ class GpioZeroborgPlugin(Plugin):
                     self.auto_mode = True
 
                 if self.auto_mode:
-                    distance = None
-                    last_recorded_distance_timestamp = None
-                    distance_record_timeout = 2.0
-
-                    while distance is None:
-                        distance = self.get_distance()
-                        logging.info('Closest obstacle distance: {} mm'.format(distance))
-
-                        if last_recorded_distance_timestamp and \
-                                time.time() - last_recorded_distance_timestamp > distance_record_timeout:
-                            # Stop the motors if we have been unable
-                            # to access the distance sensor data
-                            self._direction = None
-                            break
-
-                    last_recorded_distance_timestamp = time.time()
-
-                    if distance > 400.0:  # distance in mm
-                        self._direction = Direction.DIR_UP.value
-                    else:
-                        logging.info('Physical obstacle detected at {} mm'.
-                                     format(distance))
-                        self._direction = Direction.DIR_LEFT.value
-
+                    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