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

This commit is contained in:
Fabio Manganiello 2018-04-13 15:49:10 +02:00
parent b597097be3
commit b863213bd7
3 changed files with 51 additions and 34 deletions

View file

@ -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:

View file

@ -5,11 +5,13 @@ import time
import RPi.GPIO as gpio import RPi.GPIO as gpio
from platypush.message.response import Response from platypush.message.response import Response
from platypush.plugins import Plugin from platypush.plugins.gpio.sensor import GpioSensorPlugin
class GpioSensorDistancePlugin(Plugin): class GpioSensorDistancePlugin(GpioSensorPlugin):
def __init__(self, trigger_pin, echo_pin): def __init__(self, trigger_pin, echo_pin, *args, **kwargs):
super().__init__(*args, **kwargs)
self.trigger_pin = trigger_pin self.trigger_pin = trigger_pin
self.echo_pin = echo_pin self.echo_pin = echo_pin
self._is_probing = False self._is_probing = False
@ -20,10 +22,9 @@ class GpioSensorDistancePlugin(Plugin):
logging.info('Resetting trigger sensor on GPIO pin {}'.format(self.trigger_pin)) logging.info('Resetting trigger sensor on GPIO pin {}'.format(self.trigger_pin))
gpio.output(self.trigger_pin, False) gpio.output(self.trigger_pin, False)
time.sleep(1)
def get_distance(self): def get_measurement(self):
gpio.setmode(gpio.BCM) gpio.setmode(gpio.BCM)
gpio.setup(self.trigger_pin, gpio.OUT) gpio.setup(self.trigger_pin, gpio.OUT)
gpio.setup(self.echo_pin, gpio.IN) gpio.setup(self.echo_pin, gpio.IN)

View file

@ -37,12 +37,38 @@ class GpioZeroborgPlugin(Plugin):
self.zb.ResetEpo() self.zb.ResetEpo()
def get_distance(self): def _get_measurement(self, plugin, timeout):
distance_sensor = get_plugin('gpio.sensor.distance') measure_start_time = time.time()
if not distance_sensor: value = None
raise RuntimeError('No gpio.sensor.distance configuration found')
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): def drive(self, direction):
@ -69,30 +95,7 @@ class GpioZeroborgPlugin(Plugin):
self.auto_mode = True self.auto_mode = True
if self.auto_mode: if self.auto_mode:
distance = None self._direction = self._get_direction_from_sensors()
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
time.sleep(0.1) time.sleep(0.1)
motor_1_power = motor_2_power = motor_3_power = motor_4_power = 0.0 motor_1_power = motor_2_power = motor_3_power = motor_4_power = 0.0